From 7008ceb4539bfda74b1e5323331386a098c71f9a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 23 Jan 2020 16:42:20 -0700 Subject: [PATCH] Added PID Stuff for pigeon go straight --- src/main/java/frc4388/robot/Constants.java | 8 +- .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/commands/DriveAtVelocityPID.java | 13 +-- .../java/frc4388/robot/subsystems/Drive.java | 81 ++++++++++++++----- 4 files changed, 75 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index efcec02..f7d5130 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -28,10 +28,10 @@ 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.2, 0, 1.0); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); - public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + 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 = 15000; public static final int DRIVE_ACCELERATION = 6000; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2e99726..3d6ad6a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,13 +79,13 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveToDistancePID(m_robotDrive, 36)); + .whileHeld(new RunCommand(() -> m_robotDrive.runVelocityPID(2000), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveToDistanceMM(m_robotDrive, 36)); + .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new DriveAtVelocityPID(m_robotDrive, 12)); + .whileHeld(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive)); 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 91e6a9b..c7fa702 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -7,6 +7,8 @@ package frc4388.robot.commands; +import com.ctre.phoenix.motorcontrol.FollowerType; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; @@ -24,22 +26,23 @@ public class DriveAtVelocityPID extends CommandBase { public DriveAtVelocityPID(Drive subsystem, double targetVel) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME; + m_targetVel = targetVel /* DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;*/; addRequirements(m_drive); } // Called when the command is initially scheduled. @Override public void initialize() { - m_leftTarget = m_targetVel; - m_rightTarget = -m_targetVel; + m_leftTarget = -m_targetVel; + m_rightTarget = m_targetVel; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); + //m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); + //m_drive.runVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); + //m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); } // 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 6d5141e..99e1c4e 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,15 +7,19 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -78,6 +82,12 @@ public class Drive extends SubsystemBase { DriveConstants.REMOTE_0, // Source number [0, 1] DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + /* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */ + m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), + RemoteSensorSource.Pigeon_Yaw, + DriveConstants.REMOTE_1, + DriveConstants.DRIVE_TIMEOUT_MS); + /* Setup Sum signal to be used for Distance */ m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); @@ -88,33 +98,40 @@ public class Drive extends SubsystemBase { DriveConstants.DRIVE_TIMEOUT_MS); /* Scale Feedback by 0.5 to half the sum of Distance */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 0.5, // Coefficient + m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient DriveConstants.PID_PRIMARY, // PID Slot of Source DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - /* Scale the Feedback Sensor using a coefficient */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, + /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ + m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - - /* flip input so forward becomes back, etc */ - m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(false); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.setSensorPhase(false); - m_leftFrontMotor.setSensorPhase(false); - /* Set status frame periods to ensure we don't have stale data */ + /* Scale the Feedback Sensor using a coefficient */ + m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, + DriveConstants.PID_PRIMARY, + DriveConstants.DRIVE_TIMEOUT_MS); + + /* flip input so forward becomes back, etc */ + m_leftFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(false); + m_leftBackMotor.setInverted(InvertType.FollowMaster); + m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_rightFrontMotor.setSensorPhase(true); + m_leftFrontMotor.setSensorPhase(false); + /*m_rightBackMotor.setSensorPhase(true); + m_leftBackMotor.setSensorPhase(true);*/ + + /* Set status frame periods to ensure we don't have stale data */ m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); /* deadbands */ - m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE - m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed - + m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE + m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); //Ensures motors run at the same speed m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -149,6 +166,15 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("D Value Drive", gains.kD); SmartDashboard.putNumber("F Value Drive", gains.kF); + /** + * Max out the peak output (for all modes). + * However you can limit the output of a given PID object with configClosedLoopPeakOutput(). + */ + m_leftFrontMotor.configPeakOutputForward(+0.5, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputReverse(-0.5, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputForward(+0.5, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-0.5, DriveConstants.DRIVE_TIMEOUT_MS); + /** * 1ms per loop. PID loop can be slowed down if need be. * For example, @@ -157,15 +183,15 @@ public class Drive extends SubsystemBase { * - sensor movement is very slow causing the derivative error to be near zero. */ int closedLoopTimeMs = 1; - m_leftFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); /** * configAuxPIDPolarity(boolean invert, int timeoutMs) * false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1 * true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1 */ - m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configAuxPIDPolarity(true, DriveConstants.DRIVE_TIMEOUT_MS); } Gains gainsOld; @@ -264,17 +290,30 @@ public class Drive extends SubsystemBase { } public void runPositionPID(WPI_TalonFX talon, double targetPos) { + talon.selectProfileSlot(DriveConstants.SLOT_DISTANCE , DriveConstants.PID_PRIMARY); talon.set(TalonFXControlMode.Position, targetPos); } - public void runVelocityPID(WPI_TalonFX talon, double targetVel) { - talon.set(TalonFXControlMode.Velocity, targetVel); + public void runVelocityPID(double targetVel) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); } public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){ + talon.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); talon.set(TalonFXControlMode.MotionMagic, targetPos); } + public void runTurningPID(double targetAngle){ + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, 0, DemandType.AuxPID, targetAngle); + //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_rightFrontMotor.set(DemandType.AuxPID, 0); + } + public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr);