add comments

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
index 336c76fa4a6f3eb1e9661a9fbb7a3ca01fda65df..45966f63eedfa6984e555208b65b40107731b3c6 100644 (file)
--- a/src/main/java/frc/robot/constants/IntakeConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -3,35 +3,26 @@ package frc.robot.constants;
 public class IntakeConstants {
 
     // Motors (set actual ids)
+    /** Intake extender right motor CAN ID */
     public static final int rightID = 1;
+    /** Intake extender left motor CAN ID */
     public static final int leftID = 2;
+    /** Intake roller motor CAN ID */
     public static final int rollerID = 3;
 
-    //Motor speed
+    /** Intake roller motor speed in range [-1, 1] */
     public static final double speed = 0.2;
-    /**
-     * 12 tooth pinion driving 36 tooth driven gear
-     */
+    /** 12 tooth pinion driving 36 tooth driven gear */
     public static final double gearRatio = 36.0/12.0;
-    /**
-     * radius of the rack gear which is a 10 tooth pinion
-     */
-    public static final double radius = 0.5;
+    /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */
+    public static final double radiusRackPinion = 0.5;
 
     // Intake positions
 
-    /**
-     * max extension in inches
-     */
+    /** max extension in inches */
     public static final double maxExtension = 3.0; // 14.856; 
-    /**
-     * starting point in inches
-     */
+    /** starting point in inches */
     public static final double startingPoint = 0;
-    /**
-     * rack pitch in teeth per inch
-     */
+    /** rack pitch in teeth per inch of diameter (Diametral Pitch) DP = N teeth / Diameter in inches */
     public static final double rackPitch = 10;
-    // for simulation
-    public static final double kMaxRotations = 37.5;
 }

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index fcca87a029d6f0ae96cfd5d1cd9b8d11853f3020..9fbc76e8f0467abd4b5713e07eace8f7196cd69b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -41,7 +41,7 @@ public class Intake extends SubsystemBase {
     /** Left motor (slave) */
     private TalonFX leftMotor = new TalonFX(IntakeConstants.leftID, Constants.CANIVORE_SUB);
 
-    /** Motor characteristics for the roller motor, a Kraken X44 (aka gearbox) */
+    /** Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox) */
     private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1);
     /** Motor characteristics for the extending pair of Kraken X44 motors (aka gearbox) */
     private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2);
@@ -51,7 +51,7 @@ public class Intake extends SubsystemBase {
 
     // Use FlywheelSim for the roller
     private FlywheelSim rollerSim;
-    // for the moment of inertial, guess .5 kg at a radius of 20 mm
+    // for the moment of inertial, guess the roller is .5 kg at a radius of 20 mm
     private double moiRoller = 0.5 * 0.020 * 0.20;
     private double gearingRoller = 2.0;
 
@@ -63,10 +63,10 @@ public class Intake extends SubsystemBase {
     public Intake() {
      
         // get the maximum free speed
-        double mechFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec)/ IntakeConstants.gearRatio;
+        double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec)/ IntakeConstants.gearRatio;
 
         // this is confusing
-        maxVelocity = 0.5 * mechFreeSpeed;
+        maxVelocity = 0.5 * maxFreeSpeed;
         maxAcceleration = maxVelocity / 0.25;
 
         // Configure the motors
@@ -84,17 +84,17 @@ public class Intake extends SubsystemBase {
         // config Slot 0 PID params
         var slot0Configs = config.Slot0;
         // TODO: set PID parameters
-        slot0Configs.kP = 5;
-        slot0Configs.kI = 0;
-        slot0Configs.kD = 0;
-        slot0Configs.kV = 0;
-        slot0Configs.kA = 0;
+        slot0Configs.kP = 5.0;
+        slot0Configs.kI = 0.0;
+        slot0Configs.kD = 0.0;
+        slot0Configs.kV = 0.0;
+        slot0Configs.kA = 0.0;
 
         // configure MotionMagic
         MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
 
-        motionMagicConfigs.MotionMagicCruiseVelocity =  IntakeConstants.gearRatio * maxVelocity/IntakeConstants.radius/Math.PI/2;
-        motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.gearRatio * maxAcceleration/IntakeConstants.radius/Math.PI/2;
+        motionMagicConfigs.MotionMagicCruiseVelocity =  IntakeConstants.gearRatio * maxVelocity/IntakeConstants.radiusRackPinion/Math.PI/2;
+        motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.gearRatio * maxAcceleration/IntakeConstants.radiusRackPinion/Math.PI/2;
 
         // set the brake mode
         config.MotorOutput.withNeutralMode(NeutralModeValue.Brake);
@@ -117,6 +117,7 @@ public class Intake extends SubsystemBase {
         // extensiion is initially retracted.
         robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 0, 90, 2, new Color8Bit(255, 0, 0) ));
 
+        // add some test commands.
         SmartDashboard.putData("Extension Mechanism", mechanism);
         SmartDashboard.putData("Extend Intake", new InstantCommand(this::extend));
         SmartDashboard.putData("Retract Intake", new InstantCommand(this::retract));
@@ -126,14 +127,17 @@ public class Intake extends SubsystemBase {
         if (RobotBase.isSimulation()) {
             // build the simulation resources
 
-            // Extender
+            // Extender simulation
             // the supply voltage should change with load....
             rightMotor.getSimState().setSupplyVoltage(12.0);
 
+            // mass is just a guess
             double carriageMassKg = 3.0;
+            // rack pinion is 10 teeth and 10 DP for a radius of 1 inches
             double drumRadiusMeters = Units.inchesToMeters(1.0);
             double minHeightMeters = Units.inchesToMeters(0.0);
             double maxHeightMeters = Units.inchesToMeters(IntakeConstants.maxExtension);
+            // start retracted
             double startingHeightMeters = Units.inchesToMeters(0.0);
             intakeSim = new ElevatorSim(
                 dcMotorExtend,
@@ -145,7 +149,7 @@ public class Intake extends SubsystemBase {
                 false, 
                 startingHeightMeters);
 
-            // Roller
+            // Roller simulation
             rollerSim = new FlywheelSim(
                 LinearSystemId.createFlywheelSystem(dcMotorRoller, moiRoller, gearingRoller),
                 dcMotorRoller);
@@ -158,13 +162,14 @@ public class Intake extends SubsystemBase {
         SmartDashboard.putNumber("Intake Position:", inchExtension);
         robotExtension.setLength(inchExtension);
 
+        // Report velocity to SmartDashbboard
         // this returns rotations per second.
         double velocity = rollerMotor.getVelocity().getValueAsDouble();
         SmartDashboard.putNumber("Roller Velocity", velocity);
     }
 
     public void simulationPeriodic(){
-        // simulate the motor activity
+        // simulate the motor activities
 
         // get the applied motor voltage
         double voltage = rightMotor.getMotorVoltage().getValueAsDouble();
@@ -189,6 +194,7 @@ public class Intake extends SubsystemBase {
         voltage = rollerMotor.getMotorVoltage().getValueAsDouble();
         rollerSim.setInputVoltage(voltage);
         rollerSim.update(0.020);
+        // Sanity check:
         // the X44 has a top speed of 7530 RPM = 125 RPS
         // If the drive is 0.2, then ultimate speed should be 125 RPS * 0.2 = 25 RPS
         // result is 26 RPS.
@@ -198,8 +204,8 @@ public class Intake extends SubsystemBase {
     }
 
     /**
-     * in inches
-     * @param setpoint
+     * Set the intake extender position
+     * @param setpoint in inches
      */
     public void setPosition(double setpoint) {
         double motorRotations =inchesToRotations(setpoint);
@@ -207,7 +213,8 @@ public class Intake extends SubsystemBase {
     }
 
     /**
-     * gets motor position in inches
+     * Get the intake extender position
+     * @return inches
      */
     public double getPosition(){
         double motorRotations = rightMotor.getPosition().getValueAsDouble();
@@ -216,10 +223,11 @@ public class Intake extends SubsystemBase {
 
     /**
      * convert rotations to inches
-     * @param rotations
-     * @return
+     * @param rotations of the motor
+     * @return inches of rack travel
      */
-    public double rotationsToInches(double motorRotations){
+    public double rotationsToInches(double motorRotations) {
+        // circumference of the rack pinion
         double circ = 2 * Math.PI * 0.5;
         double pinionRotations = motorRotations / IntakeConstants.gearRatio;
         double inches = pinionRotations * circ;
@@ -228,8 +236,8 @@ public class Intake extends SubsystemBase {
 
     /**
      * convert inches to rotations
-     * @param inches
-     * @return
+     * @param inches of rack travel
+     * @return motor rotations
      */
     public double inchesToRotations(double inches){
         double circ = 2 * Math.PI * 0.5;
@@ -238,26 +246,42 @@ public class Intake extends SubsystemBase {
         return motorRotations;
     }
 
+    /**
+     * Set the roller speed.
+     * @param speed duty cycle in the range [-1, 1]
+     */
     public void spin(double speed) {
         rollerMotor.set(speed);
     }
 
+    /**
+     * Start the intake roller spinning.
+     */
     public void spinStart() {
         spin(IntakeConstants.speed);
     }
 
+    /**
+     * Stop the intake roller.
+     */
     public void spinStop() {
         spin(0.0);
     }
 
+    /** Extend the intake the maximum distance. */
     public void extend() {
        setPosition(IntakeConstants.maxExtension);
     }
 
+    /** Retract the intake to its starting position. */
     public void retract(){
         setPosition(IntakeConstants.startingPoint);
     }
 
+    /**
+     * Reclaim all the resources (e.g., motors and other devices).
+     * This step is necessary for multiple unit tests to work.
+     */
     public void close() {
         leftMotor.close();
         rightMotor.close();