From 80d52fc2092f8ce309b64bd11ae7fbd79a89c474 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 21 Jan 2019 13:14:56 -0800 Subject: [PATCH 01/12] Edited Robot Map for Arm Changed Elevator motor IDs to say Arm motor IDs --- .../src/main/java/org/usfirst/frc4388/robot/RobotMap.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 64e015f..2db0b96 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -22,8 +22,8 @@ public class RobotMap { public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; //Elevator Motors - public static final int ELEVATOR_MOTOR1_ID = 6; - public static final int ELEVATOR_MOTOR2_ID = 7; + public static final int ARM_MOTOR1_ID = 6; + public static final int ARM_MOTOR2_ID = 7; public static final int CLIMBER_CAN_ID = 10; From cf9bde39ebc9b44167bba01aad3ba8afd17143e3 Mon Sep 17 00:00:00 2001 From: Luke Staudacher Date: Mon, 21 Jan 2019 14:17:14 -0700 Subject: [PATCH 02/12] Create fsdf.code-workspace --- fsdf.code-workspace | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 fsdf.code-workspace diff --git a/fsdf.code-workspace b/fsdf.code-workspace new file mode 100644 index 0000000..d2bbe00 --- /dev/null +++ b/fsdf.code-workspace @@ -0,0 +1,7 @@ +{ + "folders": [ + { + "path": "C:\\Users\\robotics\\eclipse-workspace\\2019" + } + ] +} \ No newline at end of file From b247af6d4ff05685d5ba4b5e4a70555d274c9909 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 21 Jan 2019 13:19:39 -0800 Subject: [PATCH 03/12] Renamed Elevator Subsystem to Arm Subsystem --- 2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java | 5 +---- .../frc4388/robot/subsystems/{Elevator.java => Arm.java} | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) rename 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/{Elevator.java => Arm.java} (99%) 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 36fea3e..623cf5a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -32,10 +32,7 @@ public class Robot extends IterativeRobot - public static final Elevator elevator = new Elevator(); - - - + public static final Arm arm = new Arm(); public static final Climber climber = new Climber(); public static final Pnumatics pnumatics = new Pnumatics(); public static final long periodMS = 10; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java similarity index 99% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 4a17e98..f81e98e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -31,7 +31,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -public class Elevator extends Subsystem implements ControlLoopable +public class Arm extends Subsystem implements ControlLoopable { //PID encoder and motor private CANTalonEncoder elevatorRight; From c8c3c1489a5595376bbee58ad8b6a5f3d2fac088 Mon Sep 17 00:00:00 2001 From: Luke Staudacher Date: Mon, 21 Jan 2019 14:23:00 -0700 Subject: [PATCH 04/12] Revert "Create fsdf.code-workspace" This reverts commit cf9bde39ebc9b44167bba01aad3ba8afd17143e3. --- fsdf.code-workspace | 7 ------- 1 file changed, 7 deletions(-) delete mode 100644 fsdf.code-workspace diff --git a/fsdf.code-workspace b/fsdf.code-workspace deleted file mode 100644 index d2bbe00..0000000 --- a/fsdf.code-workspace +++ /dev/null @@ -1,7 +0,0 @@ -{ - "folders": [ - { - "path": "C:\\Users\\robotics\\eclipse-workspace\\2019" - } - ] -} \ No newline at end of file From 337bfa1778e4752833f2701f790a93a63275e1dc Mon Sep 17 00:00:00 2001 From: Daniel Williams <60667@psdschools.org> Date: Mon, 21 Jan 2019 14:57:43 -0700 Subject: [PATCH 05/12] Changed all Elevators to Arms --- .../java/org/usfirst/frc4388/robot/Robot.java | 4 +- .../org/usfirst/frc4388/robot/RobotMap.java | 2 +- .../usfirst/frc4388/robot/subsystems/Arm.java | 184 +++++++++--------- 3 files changed, 95 insertions(+), 95 deletions(-) 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) From d7d7a13d4e55c4c8639c8aa1ce70d4535407313a Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 21 Jan 2019 13:58:57 -0800 Subject: [PATCH 06/12] Edit Arm to ARM in Encoder code --- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 fd5b9f5..bc1123d 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 @@ -93,8 +93,8 @@ public class Arm extends Subsystem implements ControlLoopable try { //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); + ArmRight = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); ArmRight.setInverted(false); From 733557ecafabf2c92753a8b9ca16c691c7b8aceb Mon Sep 17 00:00:00 2001 From: Daniel Williams <60667@psdschools.org> Date: Mon, 21 Jan 2019 15:06:55 -0700 Subject: [PATCH 07/12] Arm Fix --- .../java/org/usfirst/frc4388/robot/Robot.java | 2 +- .../usfirst/frc4388/robot/subsystems/Arm.java | 79 ++++++++++--------- 2 files changed, 41 insertions(+), 40 deletions(-) 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 6e6d915..dc86600 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -152,7 +152,7 @@ public class Robot extends IterativeRobot public void updateStatus() { drive.updateStatus(operationMode); - Arm.updateStatus(operationMode); + Elevator.updateStatus(operationMode); } 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 bc1123d..dcea3cb 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 ArmRight; - private WPI_TalonSRX ArmLeft; + private CANTalonEncoder ArmMotor; + //private WPI_TalonSRX ArmLeft; //PID controller Max Scale private SoftwarePIDPositionController pidPositionControllerMaxScale; @@ -93,37 +93,38 @@ public class Arm extends Subsystem implements ControlLoopable try { //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); + ArmMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + + //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); - ArmRight.setInverted(false); + //ArmMotor.setInverted(false); //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); + //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); + //ArmLeft.setInverted(false); + //ArmLeft.setNeutralMode(NeutralMode.Brake); + ArmMotor.setNeutralMode(NeutralMode.Brake); + ArmMotor.setSensorPhase(true); //Limit Switch Left //ArmLeft.overrideLimitSwitchesEnable(true); - ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); //Limit Switch Right - //ArmRight.overrideLimitSwitchesEnable(true); - //ArmRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmMotor.overrideLimitSwitchesEnable(true); + //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); //Change This boi - // ArmRight.configForwardSoftLimitThreshold(10000, 0); //right here - //ArmRight.configReverseSoftLimitThreshold(5, 0); - //ArmRight.configForwardSoftLimitEnable(true, 0); - //ArmRight.configReverseSoftLimitEnable(true, 0); + // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here + //ArmMotor.configReverseSoftLimitThreshold(5, 0); + //ArmMotor.configForwardSoftLimitEnable(true, 0); + //ArmMotor.configReverseSoftLimitEnable(true, 0); //sos - //ArmRight.enableLimitSwitch(true, true); + //ArmMotor.enableLimitSwitch(true, true); @@ -207,7 +208,7 @@ public class Arm extends Subsystem implements ControlLoopable public void resetArmEncoder() { - ArmRight.setSelectedSensorPosition(0, 0, 0); + ArmMotor.setSelectedSensorPosition(0, 0, 0); } public void moveArmXbox() @@ -226,37 +227,37 @@ public class Arm extends Subsystem implements ControlLoopable if(ArmTuningPressed == true) { - ArmRight.set(moveArmInput * 0.5); + ArmMotor.set(moveArmInput * 0.5); } else if(ArmTuningPressed == false) { - ArmRight.set(moveArmInput); + ArmMotor.set(moveArmInput); } /* if(ArmPos <= ArmSafeZone && ArmPos >= 0) { - ArmRight.set(moveArmInput); + ArmMotor.set(moveArmInput); } else if(ArmPos > ArmSafeZone) { - ArmRight.set(moveArmInput * 0.65); + ArmMotor.set(moveArmInput * 0.65); if(holdButtonPressed == true) { - ArmRight.set(-0.43 * (0.2)); + ArmMotor.set(-0.43 * (0.2)); } else if(holdButtonPressed == false) { - ArmRight.set(moveArmInput * 0.75); + ArmMotor.set(moveArmInput * 0.75); } } else if(ArmPos < 0) { - ArmRight.set(moveArmInput * 0.75); + ArmMotor.set(moveArmInput * 0.75); } */ } @@ -268,12 +269,12 @@ public class Arm extends Subsystem implements ControlLoopable //PID encoder position public double getEncoderArmPosition() { - return ArmRight.getPositionWorld(); + return ArmMotor.getPositionWorld(); } public double getArmHeightInchesAboveFloor() { - return ArmRight.getPositionWorld(); + return ArmMotor.getPositionWorld(); } public synchronized void setControlMode(DriveControlMode controlMode) @@ -316,23 +317,23 @@ public class Arm extends Subsystem implements ControlLoopable } */ public void rawSetOutput(double output){ - ArmRight.set(/*ControlMode.PercentOutput,*/ output); + ArmMotor.set(/*ControlMode.PercentOutput,*/ output); } public void holdInPos() { - ArmRight.set(-0.43 * 0.2); + ArmMotor.set(-0.43 * 0.2); } public void stopMotors() { - ArmRight.set(0); + ArmMotor.set(0); } public void isSwitchPressed() { pressed = false; - isPressed = ArmRight.getSensorCollection(); + isPressed = ArmMotor.getSensorCollection(); if(isPressed.isFwdLimitSwitchClosed() == true) { @@ -410,10 +411,10 @@ public class Arm extends Subsystem implements ControlLoopable public void setPeriodMs(long periodMs) { //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, ArmRight); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, ArmRight); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, ArmRight); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, ArmRight); + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, ArmMotor); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, ArmMotor); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, ArmMotor); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, ArmMotor); this.periodMs = periodMs; } @@ -434,7 +435,7 @@ public class Arm extends Subsystem implements ControlLoopable { try { - SmartDashboard.putNumber("Arm Pos Ticks", ArmRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Arm Pos Ticks", ArmMotor.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } From 5fbc5c78dfd150f425cea79fb8d92836533deaee Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 21 Jan 2019 14:48:55 -0800 Subject: [PATCH 08/12] Changed Arm to arm Note: Not done editing --- .../org/usfirst/frc4388/robot/Constants.java | 10 +-- .../frc4388/robot/commands/ElevatorBasic.java | 6 +- .../usfirst/frc4388/robot/subsystems/Arm.java | 81 +++++++------------ .../frc4388/robot/subsystems/Drive.java | 1 - 4 files changed, 35 insertions(+), 63 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java index 96a6f89..f6a18a1 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -29,11 +29,11 @@ public class Constants { public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI); // Elevator - public static int kElevatorEncoderTickesPerRev = 256; - public static double kElevatorInchesOfTravelPerRev = 3.75; - public static double kElevatorEncoderTicksPerInch = 126.36; - public static double kElevatorBasicPercentOutputUp = -0.85; - public static double kElevatorBasicPercentOutputDown =.7; + public static int kArmEncoderTickesPerRev = 256; + public static double kArmInchesOfTravelPerRev = 3.75; + public static double kArmEncoderTicksPerInch = 126.36; + public static double kArmBasicPercentOutputUp = -0.85; + public static double kArmBasicPercentOutputDown =.7; // CONTROL LOOP GAINS diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java index df2b718..7d8d0fc 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java @@ -19,16 +19,16 @@ public class ElevatorBasic extends Command { public static boolean isfinishedElevatorBasic; public ElevatorBasic(double targetHeightInchesAboveFloor) { - requires(Robot.elevator); + requires(Robot.arm); m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; } // Called just before this Command runs the first time protected void initialize() { - Robot.elevator.setControlMode(DriveControlMode.RAW); + Robot.arm.setControlMode(DriveControlMode.RAW); - double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); + double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); // start out at half speed, as crude way to reduce slippage m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); 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 dcea3cb..d70b56d 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,7 +34,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class Arm extends Subsystem implements ControlLoopable { //PID encoder and motor - private CANTalonEncoder ArmMotor; + private CANTalonEncoder armMotor; //private WPI_TalonSRX ArmLeft; //PID controller Max Scale @@ -84,7 +84,7 @@ public class Arm extends Subsystem implements ControlLoopable public boolean pressed; SensorCollection isPressed; - public boolean ArmControlMode = false; + public boolean armControlMode = false; // Motor controllers //private ArrayList motorControllers = new ArrayList(); @@ -93,7 +93,7 @@ public class Arm extends Subsystem implements ControlLoopable try { //PID Arm encoder and talon - ArmMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); @@ -103,8 +103,8 @@ public class Arm extends Subsystem implements ControlLoopable //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); //ArmLeft.setInverted(false); //ArmLeft.setNeutralMode(NeutralMode.Brake); - ArmMotor.setNeutralMode(NeutralMode.Brake); - ArmMotor.setSensorPhase(true); + armMotor.setNeutralMode(NeutralMode.Brake); + armMotor.setSensorPhase(true); //Limit Switch Left //ArmLeft.overrideLimitSwitchesEnable(true); //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); @@ -208,59 +208,32 @@ public class Arm extends Subsystem implements ControlLoopable public void resetArmEncoder() { - ArmMotor.setSelectedSensorPosition(0, 0, 0); + armMotor.setSelectedSensorPosition(0, 0, 0); } public void moveArmXbox() { double moveArmInput; - double ArmSafeZone = 15; + double armSafeZone = 15; - double ArmPos = getEncoderArmPosition(); + double armPos = getEncoderArmPosition(); moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean ArmTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - if(ArmTuningPressed == true) + if(armTuningPressed == true) { - ArmMotor.set(moveArmInput * 0.5); + armMotor.set(moveArmInput * 0.5); } - else if(ArmTuningPressed == false) + else if(armTuningPressed == false) { - ArmMotor.set(moveArmInput); - } - - /* - if(ArmPos <= ArmSafeZone && ArmPos >= 0) - { - ArmMotor.set(moveArmInput); - } - else if(ArmPos > ArmSafeZone) - { - ArmMotor.set(moveArmInput * 0.65); - - - if(holdButtonPressed == true) - { - ArmMotor.set(-0.43 * (0.2)); - } - else if(holdButtonPressed == false) - { - ArmMotor.set(moveArmInput * 0.75); - } - - } - - else if(ArmPos < 0) - { - ArmMotor.set(moveArmInput * 0.75); - } - */ + armMotor.set(moveArmInput); } + } // System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range @@ -269,12 +242,12 @@ public class Arm extends Subsystem implements ControlLoopable //PID encoder position public double getEncoderArmPosition() { - return ArmMotor.getPositionWorld(); + return armMotor.getPositionWorld(); } public double getArmHeightInchesAboveFloor() { - return ArmMotor.getPositionWorld(); + return armMotor.getPositionWorld(); } public synchronized void setControlMode(DriveControlMode controlMode) @@ -317,28 +290,28 @@ public class Arm extends Subsystem implements ControlLoopable } */ public void rawSetOutput(double output){ - ArmMotor.set(/*ControlMode.PercentOutput,*/ output); + armMotor.set(/*ControlMode.PercentOutput,*/ output); } public void holdInPos() { - ArmMotor.set(-0.43 * 0.2); + armMotor.set(-0.43 * 0.2); } public void stopMotors() { - ArmMotor.set(0); + armMotor.set(0); } public void isSwitchPressed() { pressed = false; - isPressed = ArmMotor.getSensorCollection(); + isPressed = armMotor.getSensorCollection(); if(isPressed.isFwdLimitSwitchClosed() == true) { if (controlMode == DriveControlMode.JOYSTICK) { - Robot.Arm.setControlMode(DriveControlMode.STOP_MOTORS); + Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); } pressed = true; } @@ -346,7 +319,7 @@ public class Arm extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.STOP_MOTORS){ { - Robot.Arm.setControlMode(DriveControlMode.JOYSTICK); + Robot.arm.setControlMode(DriveControlMode.JOYSTICK); } pressed = false; @@ -411,10 +384,10 @@ public class Arm extends Subsystem implements ControlLoopable public void setPeriodMs(long periodMs) { //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, ArmMotor); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, ArmMotor); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, ArmMotor); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, ArmMotor); + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); this.periodMs = periodMs; } @@ -435,7 +408,7 @@ public class Arm extends Subsystem implements ControlLoopable { try { - SmartDashboard.putNumber("Arm Pos Ticks", ArmMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } 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 b984af3..3d29c64 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 @@ -210,7 +210,6 @@ public class Drive extends Subsystem implements ControlLoopable rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); // rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); - elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); //gyroPigeon = new PigeonImu(leftDrive2); gyroNavX = new AHRS(SPI.Port.kMXP); From f364065fec1e54e4e5165196830bb31bfc8d463c Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 21 Jan 2019 14:49:23 -0800 Subject: [PATCH 09/12] Saved from last change --- .../main/java/org/usfirst/frc4388/robot/Robot.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) 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 dc86600..5d66455 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(Arm); + controlLoop.addLoopable(arm); operationModeChooser = new SendableChooser(); @@ -146,14 +146,15 @@ public class Robot extends IterativeRobot LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); } - public Alliance getAlliance() { + public Alliance getAlliance() + { return m_ds.getAlliance(); } - public void updateStatus() { + public void updateStatus() + { drive.updateStatus(operationMode); - Elevator.updateStatus(operationMode); - + arm.updateStatus(operationMode); } } From c6700a3560e6ab92affc798a4903883b838abd90 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 22 Jan 2019 14:46:56 -0800 Subject: [PATCH 10/12] Changed more Elevator to arm --- .../{ElevatorBasic.java => ArmBasic.java} | 16 ++++++++-------- .../{ElevatorSetSpeed.java => ArmSetSpeed.java} | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{ElevatorBasic.java => ArmBasic.java} (87%) rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{ElevatorSetSpeed.java => ArmSetSpeed.java} (90%) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java similarity index 87% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java index 7d8d0fc..8d724c4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class ElevatorBasic extends Command { +public class ArmBasic extends Command { private double m_targetHeightInchesAboveFloor; private double m_speed; private boolean m_goingUp; @@ -18,7 +18,7 @@ public class ElevatorBasic extends Command { private double m_lastCommandExecutePeriod = 0.0; public static boolean isfinishedElevatorBasic; - public ElevatorBasic(double targetHeightInchesAboveFloor) { + public ArmBasic(double targetHeightInchesAboveFloor) { requires(Robot.arm); m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; } @@ -33,10 +33,10 @@ public class ElevatorBasic extends Command { m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); if (m_goingUp) { - m_speed = Constants.kElevatorBasicPercentOutputUp; + m_speed = Constants.kArmBasicPercentOutputUp; } else { - m_speed = Constants.kElevatorBasicPercentOutputDown; + m_speed = Constants.kArmBasicPercentOutputDown; } m_commandInitTimestamp = Timer.getFPGATimestamp(); @@ -50,14 +50,14 @@ System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targe m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; } m_lastCommandExecuteTimestamp = currentTimestamp; - Robot.elevator.rawSetOutput(m_speed); + Robot.arm.rawSetOutput(m_speed); //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { boolean finished=false; - double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); + double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0); System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor); if (remaining < 0.0) { @@ -86,9 +86,9 @@ System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , ta isfinishedElevatorBasic = isFinished(); - Robot.elevator.rawSetOutput(0.0); + Robot.arm.rawSetOutput(0.0); - Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + Robot.arm.setControlMode(DriveControlMode.JOYSTICK); } // Called when another command which requires one or more of the same diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java similarity index 90% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java index 7553d48..21e328a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java @@ -9,12 +9,12 @@ import edu.wpi.first.wpilibj.command.Command; /** * */ -public class ElevatorSetSpeed extends Command { +public class ArmSetSpeed extends Command { private double RaiseSpeed; // Constructor with speed - public ElevatorSetSpeed(double RaiseSpeed) { + public ArmSetSpeed(double RaiseSpeed) { this.RaiseSpeed = RaiseSpeed; // requires(Robot.elevatorAuton); } From bd104642aa7cd7d1d1f75e0a410dbca6d8615c0a Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 28 Jan 2019 19:00:51 -0700 Subject: [PATCH 11/12] Remove pathfinder dependency --- 2019robot/build.gradle | 1 - 1 file changed, 1 deletion(-) diff --git a/2019robot/build.gradle b/2019robot/build.gradle index f15e84c..ea2312b 100644 --- a/2019robot/build.gradle +++ b/2019robot/build.gradle @@ -50,7 +50,6 @@ dependencies { nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) testCompile 'junit:junit:4.12' - compile pathfinder() } // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') From 5bba588af98222cfc89f058a9098f0a40489b5d2 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 11 Feb 2019 17:10:17 -0700 Subject: [PATCH 12/12] Climber Updates WIP WIP --- 2019robot/build.gradle | 2 +- .../org/usfirst/frc4388/robot/RobotMap.java | 5 + .../robot/commands/InitiateClimber.java | 2 +- .../frc4388/robot/subsystems/Climber.java | 111 ++++++++++++------ 4 files changed, 84 insertions(+), 36 deletions(-) diff --git a/2019robot/build.gradle b/2019robot/build.gradle index ea2312b..23028c6 100644 --- a/2019robot/build.gradle +++ b/2019robot/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" + id "edu.wpi.first.GradleRIO" version "2019.2.1" } def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" 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 0ef7d9e..55e22a3 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -24,7 +24,12 @@ public class RobotMap { //Arm Motors public static final int ARM_MOTOR1_ID = 6; public static final int ARM_MOTOR2_ID = 7; + + //Climber Motors public static final int CLIMBER_CAN_ID = 10; + public static final int CLIMBER_WHEEL1_ID = 9; + public static final int CLIMBER_WHEEL2_ID = 11; + public static final int CLIMBER_RATCHET_ID = 12; // Pneumatics diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java index 8a6e37d..6b01543 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java @@ -15,7 +15,7 @@ public class InitiateClimber extends Command @Override protected void initialize() { - Robot.climber.setClimbSpeed(climb); + Robot.climber.setClimbSpeed(climb, speed); } @Override diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java index 3cacf76..acf990b 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -1,40 +1,78 @@ - - - - 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; import org.usfirst.frc4388.robot.commands.*; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; +import org.usfirst.frc4388.utility.PIDParams; +import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import edu.wpi.first.wpilibj.DoubleSolenoid; +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.livewindow.LiveWindow; +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.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +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; - /** * */ public class Climber extends Subsystem{ - private WPI_TalonSRX Climber; + private WPI_TalonSRX climberBack; + private WPI_TalonSRX climberFront; - public boolean on; + //Frequency Control + static float BACK_FREQ = 1; + static float FRONT_FREQ; + static float FREQ_RATIO = 0.2443744576F; + + + + //Time Control + static float TIME_TO_CLIMB = 1254; + static float TIME_TO_TILT = 1254; + static float TIME_TO_DRIVE = 1254; + static float TIME_TO_PULL = 1254; + + //Limit and Saftey vars + LimitSwitchSource limitSwitchSource; + SensorCollection isPressed; + boolean safteySwitch; public Climber(){ - try{ - - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - - } catch (Exception e) { - + climberBack = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + climberFront = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL1_ID); + climberFront2 = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL2_ID); + flipOutMotor = new WPI_TalonSRX(RobotMap.CLIMBER_RATCHET_ID); + + climber.set(ControlMode.Follower, carriageLeft.getDeviceID()); + + climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + } + catch (Exception e) { System.err.println("An error occurred in the climbing constructor"); - } + FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed } @Override @@ -42,27 +80,32 @@ public class Climber extends Subsystem{ } - @Override public void periodic() { - // Put code here to be run every loop - } - public void setClimbSpeed(boolean Climb) { - if (Climb==true) { - Climber.set(1.0); - } - if (Climb==false) { - Climber.set(0); - } -} - // Put methods for controlling this subsystem - // here. Call these from Commands. - { - // TODO Auto-generated method stub - - } + public void setClimbSpeed(boolean Climb, float speed) { + if (Climb && safteySwitch) { + if(isPressed.isFwdLimitSwitchClosed()) //If back at max height + { + climberBack.set(0); + climberFront.set(FRONT_FREQ * speed); + } + else if (isPressed.isFwdLimitSwitchClosed() == false) //If back not at max height + { + climberBack.set(BACK_FREQ * speed); + climberFront.set(FRONT_FREQ * speed); + } + } + if (Climb == false) { + climberBack.set(0); + climberFront.set(0); + } + } } - +/*TODO + * add command code for starting climb (including button press) + * add command code for flipping front out (including button press) + * add saftey switch on second controller + */