Elevator

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

@@ -57,12 +57,31 @@ public final class Constants {
     public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 23; 
     public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 3; 
 
+    public static final int TELESCOPING_MOTOR_ID = 7;
+    public static final int ARM_ROTATION_MOTOR_ID = 8;
+    public static final int ELEVATOR_CAN_ID = 9;
+    public static final int LEFT_LINK_CAN_ID = 10;
+    public static final int RIGHT_LINK_CAN_ID = 11;
+
     public static final double DRIVE_MOTOR_MIN_VOLTAGE = 0.19;
     public static final double DRIVE_MOTOR_MAX_VOLTAGE = 0.8;
     public static final double STEER_MOTOR_MIN_VOLTAGE = 0.02; 
     public static final double STEER_MOTOR_MAX_VOLTAGE = 0.5;
-    public static final double GEAR_RATIO = 6.75;
-    public static final double WHEEL_DIAMETER= 4; //inches
+    // public static final double GEAR_RATIO = 6.75;
+    public static final double ELEVATOR_SPROCKET_DIAMETER= 1.05; //inches
     public static final double TICKS_PER_REV = 2048;
-    public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; //talonfx drive encoder
+    public static final double ELEVATOR_GEAR_RATIO = 25.0; // 25 motor spins for 1 shaft spin
+
+    public static final double TICKS_PER_INCH = (TICKS_PER_REV*ELEVATOR_GEAR_RATIO)/(ELEVATOR_SPROCKET_DIAMETER*Math.PI); //talonfx drive encoder
+
+    public static final int kTimeoutMs = 1000;
+    public static final int kPIDLoopIdx = 0; //not sure what this value does or if this value should be diff for rotation, telescoping, elevator
+
+	/* Choose so that Talon does not report sensor out of phase */
+    public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”.
+
+    //public static double deadband = 0.5; //arbitrary
+
+    public static int topLimitSwitchPort = 2;
+    public static int bottomLimitSwitchPort = 1;
 }

src/main/java/frc/robot/Gains.java

@@ -0,0 +1,22 @@
+/**
+ *  Class that organizes gains used when assigning values to slots
+ */
+package frc.robot;
+
+public class Gains { //Gain is a proportional value that shows the relationship between the magnitude of an input signal to the magnitude of an output signal at steady-state.
+    public final double kP;
+    public final double kI;
+    public final double kD;
+    public final int kIzone; //can be used to auto clear the integral accumulator if the sensor pos is too far from the target. This prevent unstable oscillation if the kI is too large. Value is in sensor units.
+    public final double kPeakOutput; //Absolute max motor output during closed-loop control modes only. A value of ‘1’ represents full output in both directions.
+
+
+    public Gains(double _kP, double _kI, double _kD, int _kIzone, double _kPeakOutput){
+        kP = _kP;
+        kI = _kI;
+        kD = _kD;
+
+        kIzone = _kIzone;
+        kPeakOutput = _kPeakOutput;
+    }
+}

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

@@ -6,59 +6,39 @@
 
 import edu.wpi.first.wpilibj.TimedRobot;
 import frc.robot.subsystems.*;
-import frc.robot.OI;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.autonomous.*;
-import frc.robot.Constants;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates;
 
-import com.fasterxml.jackson.core.sym.Name;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTable.*;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
  * each mode, as described in the TimedRobot documentation. If you change the name of this class or
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
 
+/*public class Robot extends TimedRobot {
+  private static final String kDefaultAuto = "Default";
+  private static final String kCustomAuto = "My Auto";
+  private String m_autoSelected;
+  private final SendableChooser<String> m_chooser = new SendableChooser<>();
+  //private final AutonomousBase autonomousBasePD = new AutonomousBasePD(new Pose2d(0*Constants.TICKS_PER_INCH, -20*Constants.TICKS_PER_INCH, new Rotation2d(0)), 0.0, new Pose2d(), 0.0);
+  public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem();
+
+*/
 
 //private AutonomousBasePD mScore = new AutonomousBasePD(new Translation2d(222.037, 0), new Translation2d(135.091, -41.307), new Translation2d(0, -44.163), new Translation2d(222.894, -50.377), new Translation2d(0, -65.388), new Translation2d(0, -65.388));
 
 public class Robot extends TimedRobot {
-  private AutonomousBase m_autoSelected;
   private final SendableChooser<AutonomousBase> m_chooser = new SendableChooser<AutonomousBase>();
-  private AutonomousBasePD noGo = new AutonomousBasePD(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)));
-  private AutonomousBasePD placeNLeave = new AutonomousBasePD(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)));
-  private AutonomousBasePD antiCharge = new AutonomousBasePD(new Pose2d(86.840, -45.282, new Rotation2d(0)), new Pose2d(221.978, 19.463, new Rotation2d(0)), new Pose2d(135.091, -19.421, new Rotation2d(0)), new Pose2d(0, -22.277, new Rotation2d(0)), new Pose2d(222.491, -28.492, new Rotation2d(0)), new Pose2d(0, -43.502, new Rotation2d(0)), new Pose2d(0, -43.502, new Rotation2d(0)));
-  private AutonomousBasePD antiChargeOpposite = new AutonomousBasePD(new Pose2d(86.840, 45.282, new Rotation2d(0)), new Pose2d(221.978, -19.463, new Rotation2d(0)), new Pose2d(135.091, 19.421, new Rotation2d(0)), new Pose2d(0, 22.277, new Rotation2d(0)), new Pose2d(222.491, 28.492, new Rotation2d(0)), new Pose2d(0, 43.502, new Rotation2d(0)), new Pose2d(0, 43.502, new Rotation2d(0)));
-  private AutonomousBasePD engageCharge = new AutonomousBasePD(new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)));
-  private AutonomousBasePD placeTwoEngage = new AutonomousBasePD(new Pose2d(223.014, 16.468, new Rotation2d(0)), new Pose2d(0, 22.683, new Rotation2d(0)), new Pose2d(135.615, 25.539, new Rotation2d(0)), new Pose2d(222.191, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)));
-  private AutonomousBaseTimed timedPath = new AutonomousBaseTimed();
-  private AutonomousBasePD testPath = new AutonomousBasePD(new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)));
-  private AutonomousBaseMP motionProfiling = new AutonomousBaseMP(Trajectories.uno, Trajectories.dos, Trajectories.tres);
-
-  //above coordinates didn't work
-  private AutonomousBasePD leave = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)));
-  private AutonomousBasePD placeTwoEngageReal = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(257.650, 64.004, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)));
-
-  //these paths score 3 balls without touching the charge station, requires 7 Pose2ds!
-  private AutonomousBasePD threeUnderChargeStation = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(56.222, 43.068, new Rotation2d(0)), new Pose2d(197.484,45.934, new Rotation2d(0)), new Pose2d(279.077, 85.622, new Rotation2d(0)), new Pose2d(197.484,40.000, new Rotation2d(0)), new Pose2d(56.154,66.117, new Rotation2d(0)));
-  private AutonomousBasePD threeAboveChargeStation = new AutonomousBasePD(new Pose2d(56.069, 200.046, new Rotation2d(0)), new Pose2d(278.999, 180.683, new Rotation2d(0)), new Pose2d(56.069, 174.725, new Rotation2d(0)), new Pose2d(207.006, 174.725, new Rotation2d(0)), new Pose2d(278.006, 133.515, new Rotation2d(0)), new Pose2d(200.552, 185.151, new Rotation2d(0)), new Pose2d(57.062, 154.368, new Rotation2d(0)));
-
-
+
+  public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
+
   public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); //if anything breaks in the future it might be this
   private final Field2d m_field = new Field2d();
   ChassisSpeeds m_ChassisSpeeds;
@@ -67,32 +47,34 @@ public class Robot extends TimedRobot {
   public static GenericEntry kI; 
   public static GenericEntry kD; 
 
+  public static DrivetrainSubsystem getDrivetrainSubsystem(){
+    return m_drivetrainSubsystem;
+  }
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
   @Override
   public void robotInit() { //creates options for different autopaths, names are placeholders
 
-    System.out.println("#I'm Awake");
-    m_chooser.setDefaultOption("Default Auto", testPath);
-    m_chooser.addOption("My Auto 1", noGo);
-    m_chooser.addOption("My Auto 2",placeNLeave);
-    m_chooser.addOption("My Auto 3", antiCharge);
-    m_chooser.addOption("My Auto 4", antiChargeOpposite);
+    // System.out.println("#I'm Awake");
+    // m_chooser.setDefaultOption("Default Auto", testPath);
+    // m_chooser.addOption("My Auto 1", noGo);
+    // m_chooser.addOption("My Auto 2",placeNLeave);
+    // m_chooser.addOption("My Auto 3", antiCharge);
+    // m_chooser.addOption("My Auto 4", antiChargeOpposite);
     //m_chooser.addOption(name: "My Auto 5", engageCharge);
-   // m_chooser.addOption(name: "My Auto 6",placeTwoEngage);
-    m_chooser.addOption("My Auto timed", timedPath);
-    m_chooser.addOption("Motion profiling path", motionProfiling);
+    //m_chooser.addOption(name: "My Auto 6",placeTwoEngage);
+    //m_chooser.addOption("My Auto timed", timedPath);
+    //m_chooser.addOption("Motion profiling path", motionProfiling);
 
     SmartDashboard.putData("Auto choices", m_chooser);
 
-    ShuffleboardTab tab = DrivetrainSubsystem.tab;
-     kP = tab.add("Auto kP", 0.1).getEntry(); 
-     kI = tab.add("Auto kI", 0.0).getEntry();
-     kD = tab.add("Auto kD", 0.0).getEntry();
-
-
+    // ShuffleboardTab tab = DrivetrainSubsystem.tab;
+    //  kP = tab.add("Auto kP", 0.1).getEntry(); 
+    //  kI = tab.add("Auto kI", 0.0).getEntry();
+    //  kD = tab.add("Auto kD", 0.0).getEntry();
   }
 
   /**
@@ -121,18 +103,25 @@ public void robotPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    m_autoSelected = m_chooser.getSelected();
-    m_autoSelected.init();
+
+   //autonomousBasePD.init();
+
+    elevatorSubsystem.init();
+    // m_autoSelected = m_chooser.getSelected();
+    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
+    // System.out.println("Auto selected: " + m_autoSelected);
+
   }
 
   /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
 
-     m_autoSelected.periodic();
-     m_drivetrainSubsystem.drive();
-     //System.out.println("Odometry: "+ DrivetrainSubsystem.m_odometry.getPoseMeters());
-
+
+    //autonomousBasePD.periodic();
+    elevatorSubsystem.setState(ElevatorStates.SHELF_ELEVATOR_HEIGHT);
+    elevatorSubsystem.periodic();
+    //m_drivetrainSubsystem.drive();
   }
 
   /** This function is called once when teleop is enabled. */
@@ -168,17 +157,23 @@ public void disabledPeriodic() {}
   /** This function is called once when test mode is enabled. */
   @Override
   public void testInit() {
-    System.out.println("Trajectory: " + Trajectories.uno);
+    ElevatorSubsystem.elevatorMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs);
+    //m_drivetrainSubsystem.m_pose = new Pose2d(20, 30, new Rotation2d(Math.PI/4));
+    //System.out.println("m_pose: " + m_drivetrainSubsystem.m_pose);
+    //autonomousBasePD.init();
   }
 
   /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {
 
+    //elevatorSubsystem.setState(ElevatorStates.ZERO);
+    //elevatorSubsystem.periodic();
+
     /*intial test!*/
-    m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0));
+   // m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0));
 
-    m_drivetrainSubsystem.drive();
+   // m_drivetrainSubsystem.drive();
   }
 
   /** This function is called once when the robot is first started up. */
@@ -188,4 +183,4 @@ public void simulationInit() {}
   /** This function is called periodically whilst in simulation. */
   @Override
   public void simulationPeriodic() {}
-}
+}