diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d177c44..0d2305f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,9 +70,11 @@ public final class Constants { public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.0518, 0, 1.0); + /* Trajectory Constants */ - public static final double MAX_SPEED_METERS_PER_SECOND = 3; - public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3; + public static final double MAX_SPEED_METERS_PER_SECOND = 1; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; public static final double TRACK_WIDTH_METERS = 0.648; public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 72f8b0e..6b92ba8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -134,7 +134,7 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0)); + .whileHeld(new InstantCommand(() -> m_robotDrive.tankDriveVelocity(1, -1), m_robotDrive)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) @@ -265,10 +265,16 @@ public class RobotContainer { new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(0, 50) + new Translation2d(1, -1), + new Translation2d(2, 0.0), + new Translation2d(1, 1), + new Translation2d(0, 0), + new Translation2d(1, -1), + new Translation2d(2, 0.0), + new Translation2d(1, 1) ), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(50, 50, new Rotation2d(0)), + new Pose2d(0, 0, new Rotation2d(0)), // Pass config config); return exampleTrajectory; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fe897e7..609b44e 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -64,6 +64,9 @@ public class Drive extends SubsystemBase { public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH; public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; + /* Back Motor Gains */ + public static Gains m_gainsVelocityBack = DriveConstants.DRIVE_VELOCITY_GAINS_BACK; + /* Timey Whimey */ public long m_currentTimeMs = System.currentTimeMillis(); public long m_lastTimeMs = m_currentTimeMs; @@ -141,18 +144,18 @@ public class Drive extends SubsystemBase { /* PID for Back Motor Control in Tank Drive Vel */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); /* Reset Sensors for WPI_TalonFXs */ resetEncoders(); @@ -325,8 +328,8 @@ public class Drive extends SubsystemBase { m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks); m_odometry.update(Rotation2d.fromDegrees( getHeading()), - getDistanceInches(m_leftFrontMotor), - -getDistanceInches(m_rightFrontMotor)); + inchesToMeters(getDistanceInches(m_leftFrontMotor)), + -inchesToMeters(getDistanceInches(m_rightFrontMotor))); } /** @@ -439,8 +442,8 @@ public class Drive extends SubsystemBase { // m_currentAngleYaw-(360), // m_currentAngleYaw+(360)); //double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - double moveVelLeft = inchesToTicks(leftSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; - double moveVelRight = inchesToTicks(rightSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; //SmartDashboard.putNumber("Move Vel Left", moveVelLeft); //SmartDashboard.putNumber("Move Vel Right", moveVelRight); @@ -450,8 +453,6 @@ public class Drive extends SubsystemBase { m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - System.err.println(moveVelLeft); - m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight); m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft); m_leftFrontMotor.follow(m_leftBackMotor); @@ -751,8 +752,11 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - //SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + + SmartDashboard.putNumber("Left Motor Pos Meters", inchesToMeters(getDistanceInches(m_rightFrontMotor))); + SmartDashboard.putNumber("Right Motor Pos Meters", inchesToMeters(getDistanceInches(m_leftFrontMotor))); /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());