From 26a208524ee8d6b6a0b6e10a46c9006724aa2fa8 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sun, 1 Mar 2020 12:33:09 -0700 Subject: [PATCH] Finished High Gear Velocity and Turning, Working on High Gear Motion Magic --- src/main/java/frc4388/robot/Constants.java | 10 +++++----- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- .../robot/commands/DriveStraightToPositionMM.java | 14 ++++++++++++-- src/main/java/frc4388/robot/subsystems/Drive.java | 10 +++++++++- 4 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1f4cf3a..1e67858 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -52,16 +52,16 @@ public final class Constants { /* PID Constants Drive*/ public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55); public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY = 30000; public static final int DRIVE_ACCELERATION = 23000; public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4bd17d4..3c52d68 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -119,11 +119,11 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 12)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); + .whenPressed(new TurnDegrees(m_robotDrive, 90)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 925f07a..b7c74bb 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; public class DriveStraightToPositionMM extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetPosIn; double m_targetPosOut; double m_targetGyro; @@ -25,10 +27,18 @@ public class DriveStraightToPositionMM extends CommandBase { * @param subsystem drive subsystem * @param targetPos distance to travel in inches */ - public DriveStraightToPositionMM(Drive subsystem, double targetPos) { + public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + try { + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + } else { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + } + } catch (Exception e) { + System.err.println("Error In Motion Magic Switch Gains."); + } addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 8c2e2bd..058c05a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -126,7 +126,15 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); /* PID for Front Motor Control in Teleop */ - setRightMotorGains(false); + try { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { + setRightMotorGains(true); + } else { + setRightMotorGains(false); + } + } catch (Exception e) { + System.err.println("Error while trying to switch gains."); + } /* PID for Back Motor Control in Tank Drive Vel */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);