From 1dab4878f04d8619912769db46b2fd99fc5c8139 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 15 Feb 2019 17:06:04 -0700 Subject: [PATCH] Revert "Merge branch 'Wrist' of https://github.com/Team4388/2019-Hit-or-Miss into Wrist" This reverts commit 47fa9bbf877cd1bee3f63ea92527d45e8f326f3d, reversing changes made to eebb5ecedf73e27bfba66aea413eca509d708110. --- .../java/org/usfirst/frc4388/robot/Robot.java | 14 +- .../org/usfirst/frc4388/robot/RobotMap.java | 12 +- .../frc4388/robot/commands/FlipIntake.java | 53 ------ .../robot/commands/GrabBallOutOfRobot.java | 42 ++--- .../robot/commands/GrabHatchOffWall.java | 36 ---- .../commands/GrabHatchOutOfFloorIntake.java | 37 ---- .../robot/commands/RunWristMotorJumpBar.java | 53 ------ .../robot/commands/StopWristMotor.java | 52 ------ .../usfirst/frc4388/robot/subsystems/Arm.java | 161 +++++++++--------- .../frc4388/robot/subsystems/Drive.java | 111 ------------ .../frc4388/robot/subsystems/Wrist.java | 84 ++------- .../frc4388/utility/MMTalonPIDController.java | 2 +- .../frc4388/utility/PathGenerator.java | 2 +- 13 files changed, 126 insertions(+), 533 deletions(-) delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index e889ff5..e681d58 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -12,7 +12,7 @@ import org.usfirst.frc4388.robot.subsystems.Pnumatics; import org.usfirst.frc4388.robot.subsystems.Wrist; //import org.usfirst.frc4388.utility.ControlLooper; import org.usfirst.frc4388.utility.Looper; -//import org.usfirst.frc4388.utility.control.RobotStateEstimator; +import org.usfirst.frc4388.utility.control.RobotStateEstimator; import org.usfirst.frc4388.utility.math.RigidTransform2d; import org.usfirst.frc4388.utility.control.RobotState; @@ -32,7 +32,7 @@ public class Robot extends TimedRobot public static OI oi; - //public static final Drive drive = Drive.getInstance(); + public static final Drive drive = Drive.getInstance(); public static final Arm arm = Arm.getInstance(); @@ -71,9 +71,9 @@ public class Robot extends TimedRobot setPeriod(Constants.kLooperDt * 2); System.out.println("Main loop period = " + getPeriod()); oi = OI.getInstance(); - //controlLoop.register(drive); + controlLoop.register(drive); controlLoop.register(arm); - //controlLoop.register(RobotStateEstimator.getInstance()); + controlLoop.register(RobotStateEstimator.getInstance()); operationModeChooser = new SendableChooser(); operationModeChooser.addDefault("Competition", OperationMode.COMPETITION); @@ -93,7 +93,7 @@ public class Robot extends TimedRobot @Override public void disabledInit() { - //drive.setLimeLED(false); + drive.setLimeLED(false); } @Override public void disabledPeriodic() { @@ -103,7 +103,7 @@ public class Robot extends TimedRobot @Override public void autonomousInit() { controlLoop.start(); - //drive.setIsRed(getAlliance().equals(Alliance.Red)); + drive.setIsRed(getAlliance().equals(Alliance.Red)); arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES); updateChoosers(); drive.endGyroCalibration(); @@ -171,7 +171,7 @@ public class Robot extends TimedRobot public void updateStatus() { - //drive.updateStatus(operationMode); + drive.updateStatus(operationMode); arm.updateStatus(operationMode); robotState.updateStatus(operationMode); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index d925b57..efa00b7 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -2,8 +2,7 @@ package org.usfirst.frc4388.robot; -public class RobotMap -{ +public class RobotMap { // USB Port IDs public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; @@ -12,14 +11,16 @@ public class RobotMap public static final int copilot = 1; // MOTORS + public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; + public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; //carriage motors - public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6; + public static final int WRIST_LEFT_MOTOR_CAN_ID = 6; public static final int INTAKE_MOTOR = 7; //Arm Motors @@ -30,6 +31,8 @@ public class RobotMap public static final int CLIMBER_LEFT = 12; public static final int CLIMBER_RIGHT = 13; + + // Pneumatics public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; @@ -39,4 +42,7 @@ public class RobotMap public static final int LOWER_HATCH_INTAKE_PCM_ID = 5; public static final int END_EFECTOR_EXPAND_PCM_ID = 6; public static final int END_EFECTOR_CONTRACT_PCM_ID = 7; + + + } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java deleted file mode 100644 index fc232a9..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java +++ /dev/null @@ -1,53 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.*; - -import edu.wpi.first.wpilibj.command.Command; - -public class FlipIntake extends Command -{ - public FlipIntake() - { - requires(Robot.wrist); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() - { - Robot.wrist.controlPIDFlipIntake(); - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() - { - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() - { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() - { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() - { - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java index 6aa12c0..2ab8874 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -7,35 +7,29 @@ package org.usfirst.frc4388.robot.commands; -import org.usfirst.frc4388.robot.Robot; - import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; -/** -* Add your docs here. -*/ - -public class GrabBallOutOfRobot extends CommandGroup -{ +public class GrabBallOutOfRobot extends CommandGroup { + /** + * Add your docs here. + */ public GrabBallOutOfRobot() { - //Add rest of sequentials for this command group + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. - addSequential(new FlipIntake()); - //Move Arm until bar jump angle + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. - //Jump bar in robot - if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle) - { - addParallel(new WaitCommand(2)); - addSequential(new RunWristMotorJumpBar()); - addSequential(new StopWristMotor()); - } - - //continue arm movement until ball is in intake ///SENSOR for ball in robot - - //Move ball out of robot to 360 degrees - //move arm and wrist in parallel + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java deleted file mode 100644 index d7f0377..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java +++ /dev/null @@ -1,36 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4388.robot.commands; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** -* Add your docs here. -*/ - -public class GrabHatchOffWall extends CommandGroup -{ - public GrabHatchOffWall() { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java deleted file mode 100644 index b6c100c..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java +++ /dev/null @@ -1,37 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4388.robot.commands; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** -* Add your docs here. -*/ - -public class GrabHatchOutOfFloorIntake extends CommandGroup -{ - public GrabHatchOutOfFloorIntake() - { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java deleted file mode 100644 index 6fe8b19..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java +++ /dev/null @@ -1,53 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.*; - -import edu.wpi.first.wpilibj.command.Command; - -public class RunWristMotorJumpBar extends Command -{ - public RunWristMotorJumpBar() - { - requires(Robot.wrist); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() - { - Robot.wrist.jumpBar(); - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() - { - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() - { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() - { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() - { - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java deleted file mode 100644 index 164e14f..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.*; - -import edu.wpi.first.wpilibj.command.Command; - -public class StopWristMotor extends Command -{ - public StopWristMotor() - { - requires(Robot.wrist); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() - { - Robot.wrist.stopMotor(); - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() - { - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() - { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() - { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 9a97637..4b992c4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -1,7 +1,6 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.ArrayList; -import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; @@ -12,15 +11,6 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -30,9 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Arm extends Subsystem implements Loop { - //PID encoder and motor - private CANTalonEncoder armMotor; - //private WPI_TalonSRX ArmLeft; + private static Arm instance; public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; @@ -70,11 +58,10 @@ public class Arm extends Subsystem implements Loop public static final double MP_T1 = 400; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowest; - //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowest; + // Motor controllers + private ArrayList motorControllers = new ArrayList(); + private TalonSRXEncoder motor1; private TalonSRX motor2; // PID controller and params @@ -118,51 +105,67 @@ public class Arm extends Subsystem implements Loop } + catch (Exception e) { + System.err.println("An error occurred in the DriveTrain constructor"); + } + } + + @Override + public void initDefaultCommand() { + } + + public void resetZeroPosition(double position) { + mpController.resetZeroPosition(position); + } + + private synchronized void setElevatorControlMode(ArmControlMode controlMode) { + this.elevatorControlMode = controlMode; + } + + private synchronized ArmControlMode getElevatorControlMode() { + return this.elevatorControlMode; + } + + public void setSpeed(double speed) { + motor1.set(ControlMode.PercentOutput, speed); + setElevatorControlMode(ArmControlMode.MANUAL); + } + + public void setSpeedJoystick(double speed) { + motor1.set(ControlMode.PercentOutput, speed); + setElevatorControlMode(ArmControlMode.JOYSTICK_MANUAL); + } + + public void setPositionPID(double targetPositionInches) { + mpController.setPIDSlot(PID_SLOT); + updatePositionPID(targetPositionInches); + setElevatorControlMode(ArmControlMode.JOYSTICK_PID); + setFinished(false); + } + + public void updatePositionPID(double targetPositionInches) { + targetPositionInchesPID = limitPosition(targetPositionInches); + double startPositionInches = motor1.getPositionWorld(); + mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + } + + public void setPositionMP(double targetPositionInches) { + double startPositionInches = motor1.getPositionWorld(); + mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); + setFinished(false); firstMpPoint = true; setElevatorControlMode(ArmControlMode.MOTION_PROFILE); } - - - //PID encoder position - public double getEncoderArmPosition() - { - return armMotor.getPositionWorld(); - } - public double getArmHeightInchesAboveFloor() - { - return armMotor.getPositionWorld(); - } - - public synchronized void setControlMode(DriveControlMode controlMode) - { - this.controlMode = controlMode; - - isFinished = false; - } - /* - public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); - } - - public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); - } - - public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); + private double limitPosition(double targetPosition) { + if (targetPosition < MIN_POSITION_INCHES) { + return MIN_POSITION_INCHES; + } + else if (targetPosition > MAX_POSITION_INCHES) { + return MAX_POSITION_INCHES; + } + + return targetPosition; } @Override @@ -174,9 +177,11 @@ public class Arm extends Subsystem implements Loop mpController.setPID(pidPIDParamsHiGear, PID_SLOT); mpController.setPIDSlot(PID_SLOT); } - */ - public void rawSetOutput(double output){ - armMotor.set(/*ControlMode.PercentOutput,*/ output); + + @Override + public void onStop(double timestamp) { + // TODO Auto-generated method stub + } @Override @@ -204,24 +209,8 @@ public class Arm extends Subsystem implements Loop default: break; } - pressed = true; } - else - { - if (controlMode == DriveControlMode.STOP_MOTORS){ - { - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - pressed = false; - } - } - } - - //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; - - private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); @@ -269,8 +258,11 @@ public class Arm extends Subsystem implements Loop return isFinished; } - public double getPeriodMs() - { + public synchronized void setFinished(boolean isFinished) { + this.isFinished = isFinished; + } + + public double getPeriodMs() { return periodMs; } @@ -286,10 +278,15 @@ public class Arm extends Subsystem implements Loop // SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); } - catch (Exception e) - { - System.err.println("Drivetrain update status error"); + catch (Exception e) { } } + } + + public static Arm getInstance() { + if(instance == null) { + instance = new Arm(); + } + return instance; } } \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 09fca73..058869a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -930,114 +930,3 @@ public class Drive extends Subsystem implements Loop { } } - - - - - - - - - - - - - - - - - - - - -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - - - -import frc.robot.commands.DriveCommand; -import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; -import com.revrobotics.CANPIDController; -import com.revrobotics.CANEncoder; - -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.command.Subsystem; - - -/** - * Add your docs here. - */ -public class Drive extends Subsystem { - - static double LOW_GEAR_RATIO = 4.821;//6.57992 - static double HIGH_GEAR_RATIO = ; - static double CIRCUMFERENCE = 15.221; - double HIGH_INCHES_PER_REV = CIRCUMFERENCE/HIGH_GEAR_RATIO; - double LOW_INCHES_PER_REV = CIRCUMFERENCE/LOW_GEAR_RATIO; - int timeoutMs = 10; - - // Front is the follow motor, and it is based on following the primary motor of its side. - public CANSparkMax leftDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless), - leftDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless), - rightDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless), - rightDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless); - - - public CANPIDController leftPID = new CANPIDController(leftDrivePrimary); - public CANPIDController rightPID = new CANPIDController(rightDrivePrimary); - - public CANEncoder leftEncoder = new CANEncoder(leftDrivePrimary); - public CANEncoder rightEncoder = new CANEncoder(rightDrivePrimary); - - - @Override - public void initDefaultCommand() { - setDefaultCommand(new DriveCommand()); - - leftDriveFront.follow(leftDrivePrimary); - rightDriveFront.follow(rightDrivePrimary); - - leftDrivePrimary.setCANTimeout(timeoutMs); - rightDrivePrimary.setCANTimeout(timeoutMs); - leftDrivePrimary.setMotorType(MotorType.kBrushless); - rightDrivePrimary.setMotorType(MotorType.kBrushless); - - } - - public void set(double left, double right) { - leftDrivePrimary.set(left); - rightDrivePrimary.set(right); - } - - public double getLeftRevs() { - return leftEncoder.getPosition(); - } - - public double getRightRevs() { - return rightEncoder.getPosition(); - } - - public double lowinchesToRevs(double INCHES) { - return INCHES/LOW_INCHES_PER_REV; - } - public double highinchesToRevs(double INCHES) { - return INCHES/HIGH_INCHES_PER_REV; - } - - // Takes an input of ticks - public void setPID(double left, double right) { - leftPID.setReference(left, ControlType.kPosition); - rightPID.setReference(right, ControlType.kPosition); - } - - public void stop() { - leftDrivePrimary.set(0); - rightDrivePrimary.set(0); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index dfd4bb9..c936377 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -26,11 +26,6 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.command.WaitCommand; - -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -41,7 +36,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP_INTAKE, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); @@ -77,19 +72,13 @@ public class Wrist extends Subsystem public double armAngleDegrees = 90; - public static final double targetAngleDegreesBallIn = 180; //only have to flip intake and go down to get ball - ///Change values + public static final double targetAngleDegreesBallIn = -45; ///Change values public static final double targetAngleDegreesBallOut = 360; - public static final double targetAngleDegreesHatchIn = 130; + public static final double targetAngleDegreesHatchIn = 55; public static final double targetAngleDegreesHatchOut = 0; - public final double jumpBarArmAngle = -50; + public static final double jumpBarAngle = 50; //hard limit switch? public static final double armAngleForPIDSwitch = -45; ///Change values - - public static final boolean ballIntakeOut = true; - - //control mode for joystick control - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; @@ -100,18 +89,12 @@ public class Wrist extends Subsystem private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO; - LimitSwitchSource limitSwitchSource; - public Wrist() { try { //PID wrist encoder and talon - wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); - - //Limit Switch - wristRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - wristRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); } catch(Exception e) { @@ -119,17 +102,12 @@ public class Wrist extends Subsystem } } - //Jump bar by putting power to the motors for a specific amount of time - //Jump bar output - public void jumpBar() + //Flipping the intake to the other side + public void flipIntake() { - wristRight.set(0.8); - } + double currentWristAngle = wristRight.getPositionWorld(); + - //Stop wrist motor - public void stopMotor() - { - wristRight.set(0); } //Method for setting the control mode for the wrist @@ -213,29 +191,8 @@ public class Wrist extends Subsystem } else if(armAngleDegrees <= armAngleForPIDSwitch) { - if(ballIntakeOut) - { - if(armAngleDegrees > targetAngleDegreesBallIn) - { - controlPIDBallIn(); - } - else - { - controlPIDBallOut(); - } - } - else - { - if(armAngleDegrees > targetAngleDegreesHatchIn) - { - controlPIDBallIn(); - } - else - { - controlPIDHatchOut(); - } - } } + controlPID(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); @@ -265,7 +222,7 @@ public class Wrist extends Subsystem { double joystickSpeed; - joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); + joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick(joystickSpeed); } @@ -283,25 +240,6 @@ public class Wrist extends Subsystem public void controlPIDBallIn() { updatePositionPID(targetAngleDegreesBallIn); - //Needs limit to lift up (so it can get over bar) in command group - } - - public void controlPIDBallOut() - { - updatePositionPID(targetAngleDegreesBallOut); - //Needs to be tuned a lot - } - - public void controlPIDHatchIn() - { - updatePositionPID(targetAngleDegreesHatchIn); - //Needs limit to lift up (so it can get over bar) in command group - } - - public void controlPIDHatchOut() - { - updatePositionPID(targetAngleDegreesHatchOut); - //Needs to be tuned a lot } public synchronized boolean isFinished() diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java index 5f96efe..c3be5a6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java @@ -96,7 +96,7 @@ public class MMTalonPIDController // Update the set points if (controlMode == MMControlMode.STRAIGHT) { double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; - double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, 1); + double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES); rightTarget = targetValue + deltaDistance; leftTarget = targetValue - deltaDistance; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java index e39719d..f99df73 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java @@ -35,7 +35,7 @@ public class PathGenerator { Trajectory trajectory = Pathfinder.generate(points, config); centerPoints = trajectory.segments; - TankModifier modifier = new TankModifier(trajectory).modify(1); + TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES); leftPoints = modifier.getLeftTrajectory().segments; rightPoints = modifier.getRightTrajectory().segments;