diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index b821bd1..d934c82 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -15,6 +15,8 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; + + public class Drivetrain extends SubsystemBase { private SparkMax rightFront = new SparkMax (3, SparkLowLevel.MotorType.kBrushless); private SparkMax leftFront = new SparkMax (1, SparkLowLevel.MotorType.kBrushless); @@ -23,9 +25,19 @@ public class Drivetrain extends SubsystemBase { private final SparkClosedLoopController motorRightClosedLoopController; private final SparkClosedLoopController motorLeftClosedLoopController; - - private final Distance ENCODER_POSITION_FACTOR = Inches.of(8 * Math.PI * (10./52.) * (30./68.)); - private final Distance ENCODER_VELOCITY_FACTOR = Inches.of((8 * Math.PI * (10./52.) * (30./68.)) / 60.0); //inches per rotation + + /* 8 inches for diameter of wheel + multiplied by pi to get circumference + 10/52 is the gear ratio for the motor gear and gear it's connected to + 30/68 is the gear ratio for the last two gears */ + private final double WHEEL_CIRCUMFERENCE = 8 * Math.PI; + private final double MOTOR_GEAR_REDUCTION = (10./52.); + private final double FINAL_TWO_GEAR_REDUCTION = (30./68.); + + private final Distance ENCODER_POSITION_FACTOR = Inches.of(WHEEL_CIRCUMFERENCE * MOTOR_GEAR_REDUCTION * FINAL_TWO_GEAR_REDUCTION); + + //distance divided by 60 to get velocity in inches per second + private final Distance ENCODER_VELOCITY_FACTOR = Inches.of(ENCODER_POSITION_FACTOR.in(Inches)).divide(60); private final double drivetrainP = 0.08;