From 92df6a3d5f3177e56a44186f00530d93421b46f0 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sun, 1 Mar 2020 13:34:54 -0700 Subject: [PATCH] Switched All PIDs to High Gear Motion Magic and Position need testing. --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../robot/commands/DriveStraightToPositionPID.java | 14 ++++++++++++-- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1e67858..e268eb3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,7 @@ public final class Constants { 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_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); 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); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3c52d68..48a40c4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -131,7 +131,7 @@ public class RobotContainer { // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 60)); + .whenPressed(new DriveStraightToPositionPID(m_robotDrive, m_robotPneumatics, 60)); /* Driver Buttons */ // sets solenoids into high gear diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index c31944e..c8a339b 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.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 DriveStraightToPositionPID extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetPosIn; double m_targetPosOut; double m_targetGyro; @@ -24,10 +26,18 @@ public class DriveStraightToPositionPID extends CommandBase { * @param subsystem drive subsystem * @param targetPos distance to travel in inches */ - public DriveStraightToPositionPID(Drive subsystem, double targetPos) { + public DriveStraightToPositionPID(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); }