From c5011b72d70771f473f2176ee176e7e70c00948b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 18 Feb 2020 17:29:26 -0700 Subject: [PATCH] Tune the teleop controller --- src/main/java/frc4388/robot/Constants.java | 12 +++---- .../DriveWithJoystickUsingDeadAssistPID.java | 31 ++++++++++++------ .../java/frc4388/robot/subsystems/Drive.java | 32 +++++++++---------- 3 files changed, 44 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 92d6a05..4dcbaaf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,12 +29,12 @@ public final class Constants { /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.05, 0.0, 0.0, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.5); - public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY = 20000; - public static final int DRIVE_ACCELERATION = 7000; + public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.0, 0.05, 0, 0.5); + //public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + //public static final int DRIVE_CRUISE_VELOCITY = 20000; + //public static final int DRIVE_ACCELERATION = 7000; /* Trajectory Constants */ public static final double MAX_SPEED_METERS_PER_SECOND = 3; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index d0e95a6..74b662e 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -16,9 +16,10 @@ import frc4388.utility.controller.IHandController; public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { Drive m_drive; double m_targetGyro, m_currentGyro; + double m_stopPos; long m_currTime, m_deltaTime; long m_deadTimeSteer, m_deadTimeMove; - long m_deadTimeout = 500; + long m_deadTimeout = 100; IHandController m_controller; /** @@ -56,13 +57,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { /* If move stick is being used */ if (moveInput != 0) { m_deadTimeMove = m_currTime; + m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition() + + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity()); } /* If steer stick is being used */ if (steerInput != 0) { m_deadTimeSteer = m_currTime; } - /* If move stick has not been pressed for 1 sec */ + /* If move stick has been pressed within 1 sec */ if (m_currTime - m_deadTimeMove < m_deadTimeout) { /* Curves the moveInput to be slightly more gradual at first */ if (moveInput >= 0) { @@ -74,7 +77,6 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { /* If steer stick has not been used for less than 1 sec */ if (m_currTime - m_deadTimeSteer < m_deadTimeout) { runDriveWithInput(moveOutput, steerInput); - m_deadTimeSteer = m_currTime; } /* If steer stick has not been used for 1 sec */ else { @@ -103,21 +105,30 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } private void runDriveStraight(double move) { - m_drive.driveWithInputAux(move, m_targetGyro); - System.out.println("Driving with Input Aux with Target: " + m_targetGyro); + m_drive.driveWithInputAux(move * 3/4, m_targetGyro); + System.out.println("Driving Straight with Target: " + m_targetGyro); } private void runStoppedTurn(double steer) { updateGyroTarget(steer); - m_drive.runDriveVelocityPID(0, m_targetGyro); - System.out.println("Driving with Velocity PID with Target: " + m_targetGyro); + m_drive.runDrivePositionPID(m_stopPos, m_targetGyro); + + System.out.println("Turning with Target: " + m_targetGyro); + + /* if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity() > 3000) { + m_drive.driveWithInputAux(0, m_targetGyro); + System.out.println("!Turning with Target: " + m_targetGyro); + } else { + m_drive.runDriveVelocityPID(0, m_targetGyro); + System.out.println("Turning with Target: " + m_targetGyro); + }*/ } /** * If AuxPID is enabled, then update using the steer input */ private void updateGyroTarget(double steerInput) { - m_targetGyro += 2 * steerInput * m_deltaTime; + m_targetGyro -= 5 * steerInput * m_deltaTime; m_targetGyro = MathUtil.clamp( m_targetGyro, m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), @@ -128,7 +139,9 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { * set target angle to current angle (prevents buildup of gyro error). */ private void resetGyroTarget() { - m_targetGyro = m_currentGyro; + //m_targetGyro = m_currentGyro; + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1) + + (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index c18a1d6..7d96e13 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -55,7 +55,7 @@ public class Drive extends SubsystemBase { public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS; public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; - public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + //public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; public final DifferentialDriveOdometry m_odometry; @@ -101,12 +101,12 @@ public class Drive extends SubsystemBase { //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed /* PID for Front Motor Control in Teleop */ - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + //m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); @@ -122,15 +122,15 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + //m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); /* PID for Back Motor control in Auto */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);