Telescoping arm

src/main/java/frc/robot/Constants.java

@@ -69,21 +69,24 @@ public final class Constants {
     public static final double STEER_MOTOR_MAX_VOLTAGE = 0.5;
 
     public static final double SWERVE_GEAR_RATIO = 6.75;
-    public static final double TELESCOPING_ARM_GEAR_RATIO = 36.0; // as of 2/6
+    public static final double TELESCOPING_ARM_GEAR_RATIO = 36.0;
+    public static final double ELEVATOR_GEAR_RATIO = 25.0;
     public static final double FIRST_WHEEL_DIAMETER= 1.13;//0.9; //0.75 inches - rough estimate by sara 2/2
     public static final double SECOND_WHEEL_DIAMETER= 0.87;// 0.8; //0.75 inches - rough estimate by sara 2/2
     public static final double SWERVE_WHEEL_DIAMETER = 4.0;
+    public static final double ELEVATOR_SPROCKET_DIAMETER = 1.28;
 
     public static final double TICKS_PER_REV = 2048;
-    public static final double UNDER_TWO_TICKS_PER_INCH = TICKS_PER_REV*TELESCOPING_ARM_GEAR_RATIO/FIRST_WHEEL_DIAMETER/Math.PI; //talonfx drive encoder
+    public static final double ELEVATOR_TICKS_PER_INCH = TICKS_PER_REV * ELEVATOR_GEAR_RATIO/ELEVATOR_SPROCKET_DIAMETER/Math.PI;
+    public static final double UNDER_TWO_TICKS_PER_INCH = TICKS_PER_REV*TELESCOPING_ARM_GEAR_RATIO/FIRST_WHEEL_DIAMETER/Math.PI;
     public static final double OVER_TWO_TICKS_PER_INCH = TICKS_PER_REV*TELESCOPING_ARM_GEAR_RATIO/SECOND_WHEEL_DIAMETER/Math.PI;
     public static final double SWERVE_TICKS_PER_INCH = TICKS_PER_REV*SWERVE_GEAR_RATIO/SWERVE_WHEEL_DIAMETER/Math.PI;
 
-    public static final int TELESCOPING_MOTOR_ID = 29; //31;
+    public static final int TELESCOPING_MOTOR_ID = 31;
     public static final int BEAM_BREAK_RECEIVER = 0; //port number of beam break receiver
     public static final int BEAM_BREAK_SENDER = 1;
 
-    public static final int ELEVATOR_CAN_ID = 9;
+    public static final int ELEVATOR_CAN_ID = 30;
     public static final int kTimeoutMs = 1000;
     public static final int kPIDLoopIdx = 0;
 

src/main/java/frc/robot/Robot.java

@@ -28,6 +28,7 @@
 import edu.wpi.first.networktables.NetworkTable.*;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.ArmTelescopingSubsystem.TelescopingStates;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -51,6 +52,7 @@ public class Robot extends TimedRobot {
   private final SendableChooser<AutonomousBase> m_chooser = new SendableChooser<AutonomousBase>();
   public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); //if anything breaks in the future it might be this
   private static ArmTelescopingSubsystem armTelescopingSubsystem = new ArmTelescopingSubsystem();
+  private static ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
   private final Field2d m_field = new Field2d();
   double t= 0.0;
   ChassisSpeeds m_ChassisSpeeds;
@@ -142,25 +144,28 @@ public void robotPeriodic() {
   @Override
   public void autonomousInit() {
 
-    armTelescopingSubsystem.setTState(TelescopingStates.SHELF_ARM_LENGTH); //moved from auto periodic to init
+    // elevatorSubsystem.init();
+    // elevatorSubsystem.elevatorMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs);
     armTelescopingSubsystem.init();
-    armTelescopingSubsystem.telescopingMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); //VERY VERY IMPORTANT
 
     //m_drivetrainSubsystem.m_frontLeftModule.getCANCoder().getPosition();
     // System.out.println("Error code" + m_drivetrainSubsystem.m_frontLeftModule.getCANCoder().getLastError());
-    System.out.println("current pose: " + DrivetrainSubsystem.m_pose.getX() + " , " + DrivetrainSubsystem.m_pose.getY());
-    m_autoSelected = m_chooser.getSelected();
-    m_autoSelected.init();
+    // System.out.println("current pose: " + DrivetrainSubsystem.m_pose.getX() + " , " + DrivetrainSubsystem.m_pose.getY());
+    // m_autoSelected = m_chooser.getSelected();
+    // m_autoSelected.init();
 
   }
 
   /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
+    //elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT);
+    //elevatorSubsystem.periodic();
 
-     armTelescopingSubsystem.periodic();
-     m_autoSelected.periodic();
+    armTelescopingSubsystem.setTState(TelescopingStates.LOW_ARM_LENGTH); //moved from auto periodic to init
+    armTelescopingSubsystem.periodic();
 
+    //  m_autoSelected.periodic();
      //m_drivetrainSubsystem.drive();
   }
 
@@ -214,22 +219,28 @@ public void testInit() {
     // m_drivetrainSubsystem.m_pose = new Pose2d(20, 30, new Rotation2d(Math.PI/4));
     // System.out.println("m_pose: " + m_drivetrainSubsystem.m_pose);
     // autonomousBasePD.init();
+
     // armTelescopingSubsystem.init();
     // armTelescopingSubsystem.telescopingMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); //VERY VERY IMPORTANT
-    m_drivetrainSubsystem.zeroGyroscope();
 
+    //m_drivetrainSubsystem.zeroGyroscope();
+
+    armTelescopingSubsystem.init();
+    // armTelescopingSubsystem.telescopingMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); //VERY VERY IMPORTANT
   }
 
   /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {
+    // elevatorSubsystem.setState(ElevatorStates.ZERO);
+    // elevatorSubsystem.periodic();
+
     //armTelescopingSubsystem.telescopingMotor.set(ControlMode.PercentOutput, -0.1);
-    //System.out.println("telescoping ticks: " + armTelescopingSubsystem.telescopingMotor.getSelectedSensorPosition());
-    // armTelescopingSubsystem.setTState(TelescopingStates.RETRACTED);
-    // armTelescopingSubsystem.periodic();
+    armTelescopingSubsystem.setTState(TelescopingStates.RETRACTED);
+    armTelescopingSubsystem.periodic();
 
-    m_drivetrainSubsystem.setSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(0.2, 0.0, Math.toRadians(0), m_drivetrainSubsystem.getGyroscopeRotation()));
-    m_drivetrainSubsystem.drive();
+    // m_drivetrainSubsystem.setSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(0.2, 0.0, Math.toRadians(0), m_drivetrainSubsystem.getGyroscopeRotation()));
+    // m_drivetrainSubsystem.drive();
   }
 
   /** This function is called once when the robot is first started up. */

src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java

@@ -18,11 +18,10 @@ public class ArmTelescopingSubsystem {
     public TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID);
     private double startTime;
     private double desiredInches;
-    private double desiredTicks;
     public double tareEncoder;
 
     double _kP = 0.35;
-    double _kI = 0.05;
+    double _kI = 0;
     double _kD = 0;
     public int _kIzone = 0;
     public double _kPeakOutput = 1.0;
@@ -40,7 +39,7 @@ public static enum TelescopingStates{
 
     public void init(){
         System.out.println("telescoping init!! :)");
-        telescopingMotor.setInverted(false); //forward = clockwise, changed on 2/9
+        telescopingMotor.setInverted(true); //forward = clockwise, changed on 2/9
         telescopingMotor.setNeutralMode(NeutralMode.Brake);
 
         //telescopingMotor.selectProfileSlot(0, 0);
@@ -54,54 +53,55 @@ public void init(){
 
     public void periodic(){//sam requests that we can operate arm length by stick on xbox
         //telescopingMotor.set(ControlMode.PercentOutput, 0.2);
-        System.out.println("current telescoping arm motor position:" + telescopingMotor.getSelectedSensorPosition());
+        System.out.println("telescoping arm state: " + tState);
+        System.out.println("current telescoping arm motor position: " + telescopingMotor.getSelectedSensorPosition());
         if (tState == TelescopingStates.RETRACTED){
             telescopingMotor.set(ControlMode.Position, 0);
             desiredInches = 0; 
-            desiredTicks = 0;
-            telescopeDeadband();
+            telescopeDeadband(0);
         } else if (tState == TelescopingStates.LOW_ARM_LENGTH){
-            desiredInches = 3; //official 2/13
-            determineRightTicks();
+            desiredInches = 10; //official 2/13 og is 3
+            double desiredTicks = determineRightTicks(desiredInches);
             System.out.println("desired ticks: " + desiredTicks);
             telescopingMotor.set(ControlMode.Position, desiredTicks);
             System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition()));
-            telescopeDeadband();
+            telescopeDeadband(desiredTicks);
         } else if (tState == TelescopingStates.SHELF_ARM_LENGTH){
             desiredInches = 6; //official 2/13
-            determineRightTicks(); 
+            double desiredTicks = determineRightTicks(desiredInches);
             telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); // goes with 90 degrees rotation 
             System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition()));
-            telescopeDeadband();
+            telescopeDeadband(desiredTicks);
         }else if (tState == TelescopingStates.MID_ARM_LENGTH){
             desiredInches = 23; //official 2/13
-            determineRightTicks(); 
+            double desiredTicks = determineRightTicks(desiredInches);
             telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); // goes with 90 degrees rotation 
             System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition()));
-            telescopeDeadband();
+            telescopeDeadband(desiredTicks);
         }else if(tState == TelescopingStates.HIGH_ARM_LENGTH){ //high arm length
             desiredInches = 32; //official 2/13
-            determineRightTicks();
+            double desiredTicks = determineRightTicks(desiredInches);
             telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); 
-            telescopeDeadband();
+            telescopeDeadband(desiredTicks);
         }
         else { //retracted again for safety
             telescopingMotor.set(ControlMode.Position, 0);
             desiredInches = 0; 
-            desiredTicks = 0;
-            telescopeDeadband();
+            telescopeDeadband(0);
         }
     }
 
-    public void determineRightTicks(){
-        if (telescopingMotor.getSelectedSensorPosition() < 2 * Constants.UNDER_TWO_TICKS_PER_INCH){
-            desiredTicks = 2 * Constants.UNDER_TWO_TICKS_PER_INCH; 
+    public double determineRightTicks(double desiredInches){
+        if (desiredInches <= 2){
+            System.out.println("under two ticks per inch");
+            return 2 * Constants.UNDER_TWO_TICKS_PER_INCH; 
         } else{
-            desiredTicks = (desiredInches-2) * Constants.OVER_TWO_TICKS_PER_INCH;
+            System.out.println("over two ticks per inch");
+            return 2 * Constants.UNDER_TWO_TICKS_PER_INCH + (desiredInches-2) * Constants.OVER_TWO_TICKS_PER_INCH;
         }
-        System.out.println("desired ticks from inside method: " + desiredTicks);
     }
-    public void telescopeDeadband(){
+
+    public void telescopeDeadband(double desiredTicks){
         if (Math.abs(desiredTicks - telescopingMotor.getSelectedSensorPosition()) < deadband){
             telescopingMotor.set(ControlMode.PercentOutput, 0);
             System.out.println("STOPPED");
@@ -118,15 +118,19 @@ public void timedMoveArm(double time, boolean forwards){ //time in seconds
         double milliTime = time * 1000;
         System.out.println("milli time: " + milliTime);
         if(forwards == true){
-            while(System.currentTimeMillis() - startTime <= milliTime){
+            if(System.currentTimeMillis() - startTime <= milliTime){
                 telescopingMotor.set(ControlMode.PercentOutput,0.2);
             }
-            telescopingMotor.set(ControlMode.PercentOutput,0);
+            else{
+                telescopingMotor.set(ControlMode.PercentOutput,0);
+            }
         }else{
-            while(System.currentTimeMillis() - startTime <= milliTime){
+            if(System.currentTimeMillis() - startTime <= milliTime){
                 telescopingMotor.set(ControlMode.PercentOutput,-0.2);
             }
-            telescopingMotor.set(ControlMode.PercentOutput,0);
+            else{
+                telescopingMotor.set(ControlMode.PercentOutput,0);
+            }
         }
     }
 

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -13,19 +13,21 @@
 
 public class ElevatorSubsystem {
 
-    public double _kP = 0.05;
+    public double _kP = 0.15;
     public double _kI = 0.0;
     public double _kD = 0.0;
     public int _kIzone = 0;
     public double _kPeakOutput = 1.0;
 
-    public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID);
+    public TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID);
     public static ElevatorStates elevatorState = ElevatorStates.LOW_ELEVATOR_HEIGHT;
 
     // DigitalInput top_limit_switch = new DigitalInput(Constants.topLimitSwitchPort);
     // DigitalInput bottom_limit_switch = new DigitalInput(Constants.bottomLimitSwitchPort);
 
     public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput);
+    public double desiredInches;
+    public double deadband = 15000;
 
     public static enum ElevatorStates{
         ZERO, 
@@ -37,7 +39,7 @@ public static enum ElevatorStates{
 
     public void init(){
         System.out.println("elevator init!!!!");
-        elevatorMotor.setInverted(false); // looking from the front of the robot, clockwise is false (:
+        elevatorMotor.setInverted(true); // looking from the front of the robot, clockwise is false (:
         elevatorMotor.setNeutralMode(NeutralMode.Brake);
 
         //configuring deadband
@@ -48,31 +50,58 @@ public void init(){
 		elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs);
     }
 
-    public void periodic(){//still need to scale down the values or figure out why it is overshooting
-        System.out.println("elevator periodic!!!!!!!");
+    public void periodic(){ //still need to scale down the values or figure out why it is overshooting
+        System.out.println("current elevator motor position:" + elevatorMotor.getSelectedSensorPosition());
         if (elevatorState == ElevatorStates.ZERO){ //emergency stop
             elevatorMotor.set(ControlMode.Position, 0);
+            System.out.println("desired ticks: 0");
+            System.out.println("error: " + (0 - elevatorMotor.getSelectedSensorPosition()));
+            elevatorDeadband(0);
         } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){
-            elevatorMotor.set(ControlMode.Position, 5 * Constants.TICKS_PER_INCH); //official 2/13 is 5
+            desiredInches = 5; //official 2/13
+            double desiredTicks = determineRightTicks();
+            elevatorMotor.set(ControlMode.Position, desiredTicks); //official 2/13 is 5
+            System.out.println("desired ticks: " + desiredTicks);
+            System.out.println("error: " + (desiredTicks - elevatorMotor.getSelectedSensorPosition()));
+            elevatorDeadband(desiredTicks);
         } else if (elevatorState == ElevatorStates.SHELF_ELEVATOR_HEIGHT) {
-            elevatorMotor.set(ControlMode.Position, 30 * Constants.TICKS_PER_INCH); //oficial 2/13 is 39
-            System.out.println("desired ticks: " + 30*Constants.TICKS_PER_INCH);
-            System.out.println("current ticks: " + elevatorMotor.getSelectedSensorPosition());
-            System.out.println("error: " + (30*Constants.TICKS_PER_INCH - elevatorMotor.getSelectedSensorPosition()));
+            desiredInches = 30; //official 2/13
+            double desiredTicks = determineRightTicks();
+            elevatorMotor.set(ControlMode.Position, desiredTicks); //oficial 2/13 is 39
+            elevatorDeadband(desiredTicks);
         } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){
-            elevatorMotor.set(ControlMode.Position, 40 * Constants.TICKS_PER_INCH); //official 2/13
+            desiredInches = 40; //official 2/13
+            double desiredTicks = determineRightTicks();
+            elevatorMotor.set(ControlMode.Position, desiredTicks); //official 2/13
+            elevatorDeadband(desiredTicks);
         } else if(elevatorState == ElevatorStates.HIGH_ELEVATOR_HEIGHT){ //high elevator height
-            elevatorMotor.set(ControlMode.Position, 48 * Constants.TICKS_PER_INCH); //change value once we know robot dimensions
+            desiredInches = 48; //official 2/13
+            double desiredTicks = determineRightTicks();
+            elevatorMotor.set(ControlMode.Position, desiredTicks); //change value once we know robot dimensions
+            elevatorDeadband(desiredTicks);
         }
         else { //emergency stop again for safety
             elevatorMotor.set(ControlMode.Position, 0);
+            elevatorDeadband(0);
+
         }
 
         // if(top_limit_switch.get() || bottom_limit_switch.get()){
         //     setState(ElevatorStates.STOP);
         //     System.out.println("Top/bottom limit switch triggered");
         // }
     }
+    public double determineRightTicks(){
+        return desiredInches * Constants.ELEVATOR_TICKS_PER_INCH; 
+    }
+
+
+    public void elevatorDeadband(double desiredTicks){
+        if (Math.abs(desiredTicks - elevatorMotor.getSelectedSensorPosition()) < deadband){
+            elevatorMotor.set(ControlMode.PercentOutput, 0);
+            System.out.println("STOPPED");
+        }
+    }
 
     public void setState(ElevatorStates newElevatorState){
         elevatorState = newElevatorState;

src/main/java/frc/robot/subsystems/PneumaticIntakeSubsystem.java

@@ -30,7 +30,7 @@ public static enum PneumaticIntakeStates{
     //confirm we are using double solenoid
     private DoubleSolenoid solenoidOne = new DoubleSolenoid(7, PneumaticsModuleType.REVPH, 8, 10); 
     //what compressor are we using?
-    public Compressor compressor = new Compressor(PneumaticsModuleType.REVPH); 
+    // public Compressor compressor = new Compressor(PneumaticsModuleType.REVPH); //commented out for now
     // Initializes a DigitalInput on DIO 0 (roborio is built in w/ 10 DIOs (digital input-output ports))
     private DigitalInput beambreakSensor = new DigitalInput(Constants.BEAM_BREAK_RECEIVER); 
 
@@ -71,7 +71,7 @@ public void init(){
         colorMatcher.addColorMatch(kPurpleTarget);
         colorMatcher.addColorMatch(kYellowTarget);
         colorMatcher.addColorMatch(kUnknownTarget);
-        compressor.enableDigital();
+        // compressor.enableDigital();
     }
 
     public void setStatePneumaticIntake(PneumaticIntakeStates newState){
@@ -157,10 +157,10 @@ public void periodic(){
 
     }
 
-    public boolean getPSI(){
-        System.out.println(compressor.getCurrent());
-        return compressor.getPressureSwitchValue();
-    }
+    // public boolean getPSI(){
+    //     System.out.println(compressor.getCurrent());
+    //     return compressor.getPressureSwitchValue();
+    // }
 
     public void setState(PneumaticIntakeStates newPneumaticIntakeState){
         pneumaticIntakeState = newPneumaticIntakeState;