diff --git a/2019robot/build.gradle b/2019robot/build.gradle index f15e84c..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" @@ -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') 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 986f17b..395d0be 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/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 36fea3e..5d66455 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; @@ -67,7 +64,7 @@ public class Robot extends IterativeRobot oi = OI.getInstance(); controlLoop.addLoopable(drive); - controlLoop.addLoopable(elevator); + controlLoop.addLoopable(arm); operationModeChooser = new SendableChooser(); @@ -149,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); } } 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..55e22a3 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -21,10 +21,15 @@ 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 - public static final int ELEVATOR_MOTOR1_ID = 6; - public static final int ELEVATOR_MOTOR2_ID = 7; + //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/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java similarity index 83% 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 df2b718..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,25 +18,25 @@ public class ElevatorBasic extends Command { private double m_lastCommandExecutePeriod = 0.0; public static boolean isfinishedElevatorBasic; - public ElevatorBasic(double targetHeightInchesAboveFloor) { - requires(Robot.elevator); + public ArmBasic(double targetHeightInchesAboveFloor) { + 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")); 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); } 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 3be9068..021716c 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 @@ -7,14 +7,14 @@ import edu.wpi.first.wpilibj.command.Command; public class InitiateClimber extends Command { - + public InitiateClimber() { requires(Robot.climber); } @Override protected void initialize() { - Robot.climber.setClimbSpeed(kClimbLiftSpeed); + Robot.climber.setClimbSpeed(climb, speed); } @Override @@ -29,9 +29,9 @@ public class InitiateClimber extends Command // Called once after isFinished returns true protected void end() { } - + @Override protected void interrupted() { - + } -} \ No newline at end of file +} 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 57% 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..d70b56d 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,11 +31,11 @@ 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; - private WPI_TalonSRX elevatorLeft; + private CANTalonEncoder armMotor; + //private WPI_TalonSRX ArmLeft; //PID controller Max Scale private SoftwarePIDPositionController pidPositionControllerMaxScale; @@ -61,7 +61,7 @@ public class Elevator 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,47 @@ public class Elevator 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 + armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + + //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); - elevatorRight.setInverted(false); + //ArmMotor.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, ArmMotor.getDeviceID()); + //ArmLeft.setInverted(false); + //ArmLeft.setNeutralMode(NeutralMode.Brake); + armMotor.setNeutralMode(NeutralMode.Brake); + armMotor.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); + //ArmMotor.overrideLimitSwitchesEnable(true); + //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmMotor.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); + // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here + //ArmMotor.configReverseSoftLimitThreshold(5, 0); + //ArmMotor.configForwardSoftLimitEnable(true, 0); + //ArmMotor.configReverseSoftLimitEnable(true, 0); //sos - //elevatorRight.enableLimitSwitch(true, true); + //ArmMotor.enableLimitSwitch(true, true); @@ -132,7 +133,7 @@ public class Elevator 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 +201,53 @@ public class Elevator 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); + armMotor.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); + armMotor.set(moveArmInput * 0.5); } - else if(elevatorTuningPressed == false) + else if(armTuningPressed == false) { - elevatorRight.set(moveElevatorInput); - } - - /* - if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) - { - elevatorRight.set(moveElevatorInput); - } - else if(elevatorPos > elevatorSafeZone) - { - elevatorRight.set(moveElevatorInput * 0.65); - - - if(holdButtonPressed == true) - { - elevatorRight.set(-0.43 * (0.2)); - } - else if(holdButtonPressed == false) - { - elevatorRight.set(moveElevatorInput * 0.75); - } - - } - - else if(elevatorPos < 0) - { - elevatorRight.set(moveElevatorInput * 0.75); - } - */ + armMotor.set(moveArmInput); } + } -// 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 armMotor.getPositionWorld(); } - public double getElevatorHeightInchesAboveFloor() + public double getArmHeightInchesAboveFloor() { - return elevatorRight.getPositionWorld(); + return armMotor.getPositionWorld(); } public synchronized void setControlMode(DriveControlMode controlMode) @@ -283,61 +257,61 @@ public class Elevator 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); + armMotor.set(/*ControlMode.PercentOutput,*/ output); } public void holdInPos() { - elevatorRight.set(-0.43 * 0.2); + armMotor.set(-0.43 * 0.2); } public void stopMotors() { - elevatorRight.set(0); + armMotor.set(0); } public void isSwitchPressed() { pressed = false; - isPressed = elevatorRight.getSensorCollection(); + isPressed = armMotor.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 +319,7 @@ public class Elevator extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.STOP_MOTORS){ { - Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + Robot.arm.setControlMode(DriveControlMode.JOYSTICK); } pressed = false; @@ -365,31 +339,31 @@ public class Elevator 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 +384,10 @@ public class Elevator 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, armMotor); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); this.periodMs = periodMs; } @@ -434,8 +408,8 @@ public class Elevator extends Subsystem implements ControlLoopable { try { - SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor()); + SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } catch (Exception e) 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 e8046eb..16392b3 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,29 +1,80 @@ 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 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.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; - public boolean on; - + private WPI_TalonSRX climberBack; + private WPI_TalonSRX climberFront; + + //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) { - - System.err.println("An error occurred in the climbing constructor"); - + 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 @@ -31,22 +82,32 @@ public class Climber extends Subsystem{ } - @Override public void periodic() { - // Put code here to be run every loop - } - - public void setClimbSpeed(double Climb) { - Climber.set(Climb); -} - // 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 + */ 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);