From cbc112982cc09613242d703d9b95b0acc0de5b76 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 15:12:48 -0700 Subject: [PATCH] Added inches to encoder ticks conversion --- src/main/java/frc4388/robot/Constants.java | 15 ++++++++++++++- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- .../robot/commands/DriveAtVelocityPID.java | 5 ++++- .../frc4388/robot/commands/DriveToDistanceMM.java | 5 ++++- .../robot/commands/DriveToDistancePID.java | 11 ++++++++--- 5 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 466ff74..0558df5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -19,6 +19,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class DriveConstants { + /* Drive Train IDs */ public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; public static final int DRIVE_LEFT_BACK_CAN_ID = 3; @@ -31,7 +32,19 @@ public final class Constants { public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); - public static final double ENCODER_TICKS_PER_REV = 2048; + /* Drive Train Characteristics */ + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5; + public static final double WHEEL_DIAMETER_INCHES = 6; + + /* Ratio Calculation */ + public static final double TICK_TIME_TO_SECONDS = 0.1; + public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS; + public static final double WHEEL_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 73937ee..daa3e79 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -80,13 +80,13 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveToDistancePID(m_robotDrive, 40000)); + .whenPressed(new DriveToDistancePID(m_robotDrive, 36)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); + .whenPressed(new DriveToDistanceMM(m_robotDrive, 36)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 12)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.err.print("interrupt"), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index 70e54dd..91e6a9b 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; public class DriveAtVelocityPID extends CommandBase { @@ -17,11 +18,13 @@ public class DriveAtVelocityPID extends CommandBase { double m_rightTarget; /** * Creates a new DriveAtVelocityPID. + * @param subsystem drive subsystem + * @param distance target velocity in inches/second */ public DriveAtVelocityPID(Drive subsystem, double targetVel) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetVel = targetVel; + m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME; addRequirements(m_drive); } diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java index 0faf431..872a0ce 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; public class DriveToDistanceMM extends CommandBase { @@ -18,11 +19,13 @@ public class DriveToDistanceMM extends CommandBase { /** * Creates a new DriveToDistancePID. + * @param subsystem drive subsystem + * @param distance distance to travel in inches */ public DriveToDistanceMM(Drive subsystem, double distance) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_distance = distance; + m_distance = distance * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); } diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 3fe0ea0..ab46b5d 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -9,6 +9,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; public class DriveToDistancePID extends CommandBase { @@ -19,12 +20,15 @@ public class DriveToDistancePID extends CommandBase { /** * Creates a new DriveToDistancePID. + * @param subsystem drive subsystem + * @param distance distance to travel in inches */ public DriveToDistancePID(Drive subsystem, double distance) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_distance = distance; + m_distance = distance * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); + SmartDashboard.putNumber("Distance Target Inches", distance); } // Called when the command is initially scheduled. @@ -32,8 +36,9 @@ public class DriveToDistancePID extends CommandBase { public void initialize() { m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - SmartDashboard.putNumber("Left Target", m_leftTarget); - SmartDashboard.putNumber("Right Target", m_rightTarget); + SmartDashboard.putNumber("Distance Target Ticks", m_distance); + SmartDashboard.putNumber("Left Target Ticks", m_leftTarget); + SmartDashboard.putNumber("Right Target Ticks", m_rightTarget); } // Called every time the scheduler runs while the command is scheduled.