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 623cf5a..6e6d915 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -64,7 +64,7 @@ public class Robot extends IterativeRobot oi = OI.getInstance(); controlLoop.addLoopable(drive); - controlLoop.addLoopable(elevator); + controlLoop.addLoopable(Arm); operationModeChooser = new SendableChooser(); @@ -152,7 +152,7 @@ public class Robot extends IterativeRobot public void updateStatus() { drive.updateStatus(operationMode); - elevator.updateStatus(operationMode); + Arm.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 2db0b96..0ef7d9e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -21,7 +21,7 @@ public class RobotMap { public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; - //Elevator Motors + //Arm Motors public static final int ARM_MOTOR1_ID = 6; public static final int ARM_MOTOR2_ID = 7; public static final int CLIMBER_CAN_ID = 10; 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 f81e98e..fd5b9f5 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 @@ -34,8 +34,8 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class Arm extends Subsystem implements ControlLoopable { //PID encoder and motor - private CANTalonEncoder elevatorRight; - private WPI_TalonSRX elevatorLeft; + private CANTalonEncoder ArmRight; + private WPI_TalonSRX ArmLeft; //PID controller Max Scale private SoftwarePIDPositionController pidPositionControllerMaxScale; @@ -61,7 +61,7 @@ public class Arm extends Subsystem implements ControlLoopable private double targetPPosition; //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch; + public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; //control mode for joystick control private DriveControlMode controlMode = DriveControlMode.JOYSTICK; @@ -84,46 +84,46 @@ public class Arm extends Subsystem implements ControlLoopable public boolean pressed; SensorCollection isPressed; - public boolean elevatorControlMode = false; + public boolean ArmControlMode = false; // Motor controllers //private ArrayList motorControllers = new ArrayList(); - public Elevator() + public Arm() { try { - //PID elevator encoder and talon - elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); - elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID); + //PID Arm encoder and talon + ArmRight = new CANTalonEncoder(RobotMap.Arm_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + ArmLeft = new WPI_TalonSRX(RobotMap.Arm_MOTOR2_ID); - elevatorRight.setInverted(false); + ArmRight.setInverted(false); - //Setting left elevator motor as follower - elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID()); - elevatorLeft.setInverted(false); - elevatorLeft.setNeutralMode(NeutralMode.Brake); - elevatorRight.setNeutralMode(NeutralMode.Brake); - elevatorRight.setSensorPhase(true); + //Setting left Arm motor as follower + ArmLeft.set(ControlMode.Follower, ArmRight.getDeviceID()); + ArmLeft.setInverted(false); + ArmLeft.setNeutralMode(NeutralMode.Brake); + ArmRight.setNeutralMode(NeutralMode.Brake); + ArmRight.setSensorPhase(true); //Limit Switch Left - //elevatorLeft.overrideLimitSwitchesEnable(true); - elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmLeft.overrideLimitSwitchesEnable(true); + ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); //Limit Switch Right - //elevatorRight.overrideLimitSwitchesEnable(true); - //elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmRight.overrideLimitSwitchesEnable(true); + //ArmRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); //Change This boi - // elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here - //elevatorRight.configReverseSoftLimitThreshold(5, 0); - //elevatorRight.configForwardSoftLimitEnable(true, 0); - //elevatorRight.configReverseSoftLimitEnable(true, 0); + // ArmRight.configForwardSoftLimitThreshold(10000, 0); //right here + //ArmRight.configReverseSoftLimitThreshold(5, 0); + //ArmRight.configForwardSoftLimitEnable(true, 0); + //ArmRight.configReverseSoftLimitEnable(true, 0); //sos - //elevatorRight.enableLimitSwitch(true, true); + //ArmRight.enableLimitSwitch(true, true); @@ -132,7 +132,7 @@ public class Arm extends Subsystem implements ControlLoopable } catch(Exception e) { - System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor"); + System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); } } @@ -200,80 +200,80 @@ public class Arm extends Subsystem implements ControlLoopable return move; } - public void setElevatorMode() + public void setArmMode() { setControlMode(DriveControlMode.JOYSTICK); } - public void resetElevatorEncoder() + public void resetArmEncoder() { - elevatorRight.setSelectedSensorPosition(0, 0, 0); + ArmRight.setSelectedSensorPosition(0, 0, 0); } - public void moveElevatorXbox() + public void moveArmXbox() { - double moveElevatorInput; - double elevatorSafeZone = 15; + double moveArmInput; + double ArmSafeZone = 15; - double elevatorPos = getEncoderElevatorPosition(); + double ArmPos = getEncoderArmPosition(); - moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis(); + moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); - //double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY); + //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + boolean ArmTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - if(elevatorTuningPressed == true) + if(ArmTuningPressed == true) { - elevatorRight.set(moveElevatorInput * 0.5); + ArmRight.set(moveArmInput * 0.5); } - else if(elevatorTuningPressed == false) + else if(ArmTuningPressed == false) { - elevatorRight.set(moveElevatorInput); + ArmRight.set(moveArmInput); } /* - if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) + if(ArmPos <= ArmSafeZone && ArmPos >= 0) { - elevatorRight.set(moveElevatorInput); + ArmRight.set(moveArmInput); } - else if(elevatorPos > elevatorSafeZone) + else if(ArmPos > ArmSafeZone) { - elevatorRight.set(moveElevatorInput * 0.65); + ArmRight.set(moveArmInput * 0.65); if(holdButtonPressed == true) { - elevatorRight.set(-0.43 * (0.2)); + ArmRight.set(-0.43 * (0.2)); } else if(holdButtonPressed == false) { - elevatorRight.set(moveElevatorInput * 0.75); + ArmRight.set(moveArmInput * 0.75); } } - else if(elevatorPos < 0) + else if(ArmPos < 0) { - elevatorRight.set(moveElevatorInput * 0.75); + ArmRight.set(moveArmInput * 0.75); } */ } -// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range +// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range //PID encoder position - public double getEncoderElevatorPosition() + public double getEncoderArmPosition() { - return elevatorRight.getPositionWorld(); + return ArmRight.getPositionWorld(); } - public double getElevatorHeightInchesAboveFloor() + public double getArmHeightInchesAboveFloor() { - return elevatorRight.getPositionWorld(); + return ArmRight.getPositionWorld(); } public synchronized void setControlMode(DriveControlMode controlMode) @@ -283,61 +283,61 @@ public class Arm extends Subsystem implements ControlLoopable isFinished = false; } /* - public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError) + public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) { - double elevatorTargetPos = 0; - this.targetPPosition = elevatorTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? - Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); } - public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError) + public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) { - double elevatorTargetPos = 0; - this.targetPPosition = elevatorTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); - Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); } - public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError) + public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) { - double elevatorTargetPos = 0; - this.targetPPosition = elevatorTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); - Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); } - public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError) + public void setArmPIDLowest(double ArmPosition, double maxError, double minError) { - double elevatorTargetPos = 0; - this.targetPPosition = elevatorTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); - Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); } */ public void rawSetOutput(double output){ - elevatorRight.set(/*ControlMode.PercentOutput,*/ output); + ArmRight.set(/*ControlMode.PercentOutput,*/ output); } public void holdInPos() { - elevatorRight.set(-0.43 * 0.2); + ArmRight.set(-0.43 * 0.2); } public void stopMotors() { - elevatorRight.set(0); + ArmRight.set(0); } public void isSwitchPressed() { pressed = false; - isPressed = elevatorRight.getSensorCollection(); + isPressed = ArmRight.getSensorCollection(); if(isPressed.isFwdLimitSwitchClosed() == true) { if (controlMode == DriveControlMode.JOYSTICK) { - Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS); + Robot.Arm.setControlMode(DriveControlMode.STOP_MOTORS); } pressed = true; } @@ -345,7 +345,7 @@ public class Arm extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.STOP_MOTORS){ { - Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + Robot.Arm.setControlMode(DriveControlMode.JOYSTICK); } pressed = false; @@ -365,31 +365,31 @@ public class Arm extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { - moveElevatorXbox(); + moveArmXbox(); } else if (!isFinished) { //PID control mode if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) { - isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition()); + isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition()); } else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) { - isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition()); + isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition()); } else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) { - isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition()); + isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition()); } else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); } /* else if(controlMode == DriveControlMode.RAW) { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); } */ } @@ -410,10 +410,10 @@ public class Arm extends Subsystem implements ControlLoopable public void setPeriodMs(long periodMs) { //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight); + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, ArmRight); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, ArmRight); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, ArmRight); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, ArmRight); this.periodMs = periodMs; } @@ -434,8 +434,8 @@ public class Arm extends Subsystem implements ControlLoopable { try { - SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor()); + SmartDashboard.putNumber("Arm Pos Ticks", ArmRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } catch (Exception e)