From 18c21fdb7e84dc713e2e4250852d77c1be472821 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 13 Jan 2020 17:53:42 -0700 Subject: [PATCH 01/49] Added motion magic stuff Looked at the CTRE website and went to github. Added the stuff we saw on there for motion magic. Needs to be tested to see if it's accurate. --- src/main/java/frc4388/robot/Constants.java | 11 +++++++ src/main/java/frc4388/robot/Gains.java | 30 +++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 3 ++ .../java/frc4388/robot/subsystems/Drive.java | 27 +++++++++-------- 4 files changed, 58 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc4388/robot/Gains.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a0385ee..6ba3ae9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,6 +24,17 @@ public final class Constants { public static final int DRIVE_LEFT_BACK_CAN_ID = 3; public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; + + /* PID Constants */ + public static final int SLOT_IDX = 0; + + public static final int PID_LOOP_IDX = 0; + + public static final int TIMEOUT_MS = 30; + + static final Gains kGains = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/robot/Gains.java new file mode 100644 index 0000000..14ede74 --- /dev/null +++ b/src/main/java/frc4388/robot/Gains.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +/** + * Add your docs here. + */ +public class Gains { + public final double kP; + public final double kI; + public final double kD; + public final double kF; + public final int kIzone; + public final double kPeakOutput; + + public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput) + { + kP = _kP; + kI = _kI; + kD = _kD; + kF = _kF; + kIzone = _kIzone; + kPeakOutput = _kPeakOutput; + } +} diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ad837e9..72d42fe 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,6 +69,9 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .whenPressed(() -> m_robotDrive.goToTargetPos()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index c8d4d1e..fe12acd 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -22,11 +24,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import frc4388.robot.Constants.DriveConstants; +import frc4388.utility.controller.XboxController; /** * Add your docs here. */ -public class Drive extends ProfiledPIDSubsystem { +public class Drive extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. @@ -42,9 +45,6 @@ public class Drive extends ProfiledPIDSubsystem { * Add your docs here. */ public Drive() { - /* */ - super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0); - /* factory default values */ m_leftFrontMotor.configFactoryDefault(); m_rightFrontMotor.configFactoryDefault(); @@ -65,6 +65,10 @@ public class Drive extends ProfiledPIDSubsystem { m_rightFrontMotor.setInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); + + /* Motor Encoders */ + m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS); + m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS); } @Override @@ -121,15 +125,12 @@ public class Drive extends ProfiledPIDSubsystem { m_pigeon.setAccumZAngle(0); } - @Override - protected void useOutput(double output, TrapezoidProfile.State setpoint) { - // TODO Auto-generated method stub - - } + // Motion Magic Testing + public void goToTargetPos() + { + double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0; - @Override - protected double getMeasurement() { - // TODO Auto-generated method stub - return 0; + m_leftFrontMotor.set(ControlMode.MotionMagic, targetPos); + m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos); } } From 097f7414dba0a8e6ce5fb92f6e9a8da20cdb4414 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 13 Jan 2020 19:26:25 -0700 Subject: [PATCH 02/49] Add Configs for Drive PID --- src/main/java/frc4388/robot/Constants.java | 13 +++---- .../java/frc4388/robot/subsystems/Drive.java | 36 +++++++++++++++++-- 2 files changed, 39 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6ba3ae9..b7a4e4c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,14 +25,11 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; - /* PID Constants */ - public static final int SLOT_IDX = 0; - - public static final int PID_LOOP_IDX = 0; - - public static final int TIMEOUT_MS = 30; - - static final Gains kGains = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + /* PID Constants Drive*/ + public static final int DRIVE_SLOT_IDX = 0; + public static final int DRIVE_PID_LOOP_IDX = 0; + public static final int DRIVE_TIMEOUT_MS = 30; + public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fe12acd..ab834c7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.PigeonIMU; @@ -67,8 +68,39 @@ public class Drive extends SubsystemBase { m_rightBackMotor.setInverted(InvertType.FollowMaster); /* Motor Encoders */ - m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS); - m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS); + m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); + m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); + m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kD, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); + + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); } @Override From d158c008716f2c7c486b0ddaee2a03bdb6d4a10b Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 13 Jan 2020 19:34:25 -0700 Subject: [PATCH 03/49] Display Gains Values on Smart Dashboard --- src/main/java/frc4388/robot/subsystems/Drive.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index ab834c7..84819f5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -109,10 +109,19 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + + SmartDashboard.putNumber("P Value", DriveConstants.DRIVE_GAINS.kP); + SmartDashboard.putNumber("I Value", DriveConstants.DRIVE_GAINS.kI); + SmartDashboard.putNumber("D Value", DriveConstants.DRIVE_GAINS.kD); + SmartDashboard.putNumber("F Value", DriveConstants.DRIVE_GAINS.kF); + } catch (Exception e) { - System.err.println("The operation failed successfully."); + + System.err.println("The programming team has failed successfully in the Drive Subsystem."); + } } From 3c0a64ffde0ea2af4538bcd12fc6159371c041b5 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 13 Jan 2020 19:45:10 -0700 Subject: [PATCH 04/49] Changed Mistake Stuffff --- src/main/java/frc4388/robot/subsystems/Drive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 84819f5..966da67 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -113,10 +113,10 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("P Value", DriveConstants.DRIVE_GAINS.kP); - SmartDashboard.putNumber("I Value", DriveConstants.DRIVE_GAINS.kI); - SmartDashboard.putNumber("D Value", DriveConstants.DRIVE_GAINS.kD); - SmartDashboard.putNumber("F Value", DriveConstants.DRIVE_GAINS.kF); + SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); } catch (Exception e) { From 928fb6cd5ad2a1f6f151e2877427976b81311f66 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 13 Jan 2020 19:55:53 -0700 Subject: [PATCH 05/49] Input/Output Smart Dashboard PID Values --- .../java/frc4388/robot/subsystems/Drive.java | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 966da67..babc27d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -101,22 +101,35 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Smart Dashboard Initial Values */ + SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + + SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); } @Override public void periodic() { try { - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.getNumber("Pigeon Yaw", getGyroYaw()); + SmartDashboard.getNumber("Pigeon Pitch", getGyroPitch()); + SmartDashboard.getNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.getNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.getNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); } catch (Exception e) { From aa1fbe2e75877b46e78c7d6ed46b8a8d0add3c11 Mon Sep 17 00:00:00 2001 From: Aarav <91212@psdschools.org> Date: Thu, 16 Jan 2020 16:47:17 -0700 Subject: [PATCH 06/49] Testing changes to pid --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 1 + src/main/java/frc4388/robot/subsystems/Drive.java | 8 +++++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b7a4e4c..e40aaaf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,7 +29,7 @@ public final class Constants { public static final int DRIVE_SLOT_IDX = 0; public static final int DRIVE_PID_LOOP_IDX = 0; public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains DRIVE_GAINS = new Gains(10.0, 0.0, 0.0, 0.2, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 72d42fe..7264254 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -70,6 +70,7 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(() -> m_robotDrive.goToTargetPos()); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index babc27d..f0f5bf5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -114,6 +114,12 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + int closedLoopTimeMs = 1; + m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + + } @Override @@ -185,6 +191,6 @@ public class Drive extends SubsystemBase { double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0; m_leftFrontMotor.set(ControlMode.MotionMagic, targetPos); - m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos); + m_rightFrontMotor.follow(m_leftFrontMotor); } } From 69b28c73b3b53b88b40e17fce619486f140bf7ae Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 16 Jan 2020 17:56:18 -0700 Subject: [PATCH 07/49] PID work --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 4 +-- .../java/frc4388/robot/subsystems/Drive.java | 28 +++++++++++-------- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e40aaaf..3a18d3a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,7 +29,7 @@ public final class Constants { public static final int DRIVE_SLOT_IDX = 0; public static final int DRIVE_PID_LOOP_IDX = 0; public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_GAINS = new Gains(10.0, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7264254..77fd71f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,7 +62,7 @@ public class RobotContainer { /* Driver Buttons */ // test command to spin the robot while pressing A on the driver controller new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(() -> m_robotDrive.driveWithInput(0, 1)); + .whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive); /* Operator Buttons */ // activates "Lit Mode" @@ -72,7 +72,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(() -> m_robotDrive.goToTargetPos()); + .whileHeld(() -> m_robotDrive.goToTargetPos(), m_robotDrive); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index f0f5bf5..3218144 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -12,6 +12,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.PigeonIMU; @@ -68,8 +69,8 @@ public class Drive extends SubsystemBase { m_rightBackMotor.setInverted(InvertType.FollowMaster); /* Motor Encoders */ - m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); @@ -107,8 +108,10 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); @@ -125,12 +128,14 @@ public class Drive extends SubsystemBase { @Override public void periodic() { try { - SmartDashboard.getNumber("Pigeon Yaw", getGyroYaw()); - SmartDashboard.getNumber("Pigeon Pitch", getGyroPitch()); - SmartDashboard.getNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.getNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.getNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); @@ -188,9 +193,10 @@ public class Drive extends SubsystemBase { // Motion Magic Testing public void goToTargetPos() { - double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0; + //double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0; + double targetPos = 1000; - m_leftFrontMotor.set(ControlMode.MotionMagic, targetPos); - m_rightFrontMotor.follow(m_leftFrontMotor); + m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetPos); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, -targetPos); } } From 686e9feb006b35d5887dba5050e79cdb7ac86ac7 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 16 Jan 2020 18:22:01 -0700 Subject: [PATCH 08/49] Make gains editable in SmartDashboard --- src/main/java/frc4388/robot/Gains.java | 12 +++--- .../java/frc4388/robot/subsystems/Drive.java | 42 ++++++++++++------- 2 files changed, 33 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/robot/Gains.java index 14ede74..41c6422 100644 --- a/src/main/java/frc4388/robot/Gains.java +++ b/src/main/java/frc4388/robot/Gains.java @@ -11,12 +11,12 @@ package frc4388.robot; * Add your docs here. */ public class Gains { - public final double kP; - public final double kI; - public final double kD; - public final double kF; - public final int kIzone; - public final double kPeakOutput; + public double kP; + public double kI; + public double kD; + public double kF; + public int kIzone; + public double kPeakOutput; public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput) { diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3218144..40af86d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; - +import frc4388.robot.Gains; import frc4388.robot.Constants.DriveConstants; import frc4388.utility.controller.XboxController; @@ -43,6 +43,8 @@ public class Drive extends SubsystemBase { public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public static Gains m_gains = DriveConstants.DRIVE_GAINS; + /** * Add your docs here. */ @@ -84,16 +86,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kD, DriveConstants.DRIVE_TIMEOUT_MS); + setDriveTrainGains(); m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); @@ -137,10 +130,12 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + m_gains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + m_gains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + m_gains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + m_gains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + setDriveTrainGains(); } catch (Exception e) { @@ -160,6 +155,23 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setNeutralMode(mode); } + /** + * Add your docs here. + */ + public void setDriveTrainGains(){ + m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); + m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); + m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + } + /** * Add your docs here. */ From 6fc414ccb88303a68c406ec773c31127c7fba1c3 Mon Sep 17 00:00:00 2001 From: Aarav <91212@psdschools.org> Date: Thu, 16 Jan 2020 20:11:48 -0700 Subject: [PATCH 09/49] Add Distance PID Command --- .../java/frc4388/robot/RobotContainer.java | 3 +- .../robot/commands/DriveToDistancePID.java | 57 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 18 +++--- 3 files changed, 67 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveToDistancePID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 77fd71f..a06bda1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; @@ -72,7 +73,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(() -> m_robotDrive.goToTargetPos(), m_robotDrive); + .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java new file mode 100644 index 0000000..6512150 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveToDistancePID extends CommandBase { + Drive m_drive; + double m_distance; + double m_leftTarget; + double m_rightTarget; + + /** + * Creates a new DriveToDistancePID. + */ + public DriveToDistancePID(Drive subsystem, double distance) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_distance = distance; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; + m_rightTarget = m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.gotoPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.gotoPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 40af86d..d411fa1 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -179,6 +179,14 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } + public void gotoPositionPID(WPI_TalonFX talon, double targetPos) { + talon.set(TalonFXControlMode.Position, targetPos); + } + + public void gotoVelocityPID(WPI_TalonFX talon, double targetVel) { + talon.set(TalonFXControlMode.Velocity, targetVel); + } + public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); @@ -201,14 +209,4 @@ public class Drive extends SubsystemBase { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); } - - // Motion Magic Testing - public void goToTargetPos() - { - //double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0; - double targetPos = 1000; - - m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetPos); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, -targetPos); - } } From 9be99be0c303a376eb7917da3d1f182e4987c7b0 Mon Sep 17 00:00:00 2001 From: Aarav <91212@psdschools.org> Date: Thu, 16 Jan 2020 20:22:50 -0700 Subject: [PATCH 10/49] Add PID and Motion Magic Commands --- .../java/frc4388/robot/RobotContainer.java | 4 ++ .../robot/commands/DriveToDistanceMM.java | 57 +++++++++++++++++++ .../robot/commands/DriveToDistancePID.java | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 12 ++-- 4 files changed, 71 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a06bda1..6469cc8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.LED; @@ -74,6 +75,9 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); + + new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java new file mode 100644 index 0000000..c259e54 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveToDistanceMM extends CommandBase { + Drive m_drive; + double m_distance; + double m_leftTarget; + double m_rightTarget; + + /** + * Creates a new DriveToDistancePID. + */ + public DriveToDistanceMM(Drive subsystem, double distance) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_distance = distance; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; + m_rightTarget = m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 6512150..e0bbbe8 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -36,8 +36,8 @@ public class DriveToDistancePID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.gotoPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.gotoPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); + m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); } // 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 d411fa1..3b81cfc 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -74,7 +74,7 @@ public class Drive extends SubsystemBase { //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); + /*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); @@ -84,7 +84,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/ setDriveTrainGains(); @@ -179,14 +179,18 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void gotoPositionPID(WPI_TalonFX talon, double targetPos) { + public void runPositionPID(WPI_TalonFX talon, double targetPos) { talon.set(TalonFXControlMode.Position, targetPos); } - public void gotoVelocityPID(WPI_TalonFX talon, double targetVel) { + public void runVelocityPID(WPI_TalonFX talon, double targetVel) { talon.set(TalonFXControlMode.Velocity, targetVel); } + public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){ + talon.set(TalonFXControlMode.MotionMagic, targetPos); + } + public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); From b22386e5922b0fa22e48472b947fa3fd2b539db2 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 17 Jan 2020 17:28:51 -0700 Subject: [PATCH 11/49] Invert PID target so robot no spin --- src/main/java/frc4388/robot/commands/DriveToDistanceMM.java | 2 +- src/main/java/frc4388/robot/commands/DriveToDistancePID.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java index c259e54..0faf431 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -30,7 +30,7 @@ public class DriveToDistanceMM extends CommandBase { @Override public void initialize() { m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance; + m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index e0bbbe8..6c8e1cc 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -30,7 +30,7 @@ public class DriveToDistancePID extends CommandBase { @Override public void initialize() { m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance; + m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); } // Called every time the scheduler runs while the command is scheduled. From 5adb675ef4025321dec31e2b8d0435492de6d753 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 17 Jan 2020 17:30:04 -0700 Subject: [PATCH 12/49] Remove uneeded imports and static declarations in Drive --- .../java/frc4388/robot/subsystems/Drive.java | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3b81cfc..2ad83d9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,26 +7,17 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.PigeonIMU; import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -import frc4388.robot.Gains; import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.controller.XboxController; +import frc4388.robot.Gains; /** * Add your docs here. @@ -35,13 +26,13 @@ public class Drive extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. - public static WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public static WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public static WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public static WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); + public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); + public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); + public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); - public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); public static Gains m_gains = DriveConstants.DRIVE_GAINS; From 4d7a03d230616fa3324252ed569aa71c3ef1ace2 Mon Sep 17 00:00:00 2001 From: Aarav <91212@psdschools.org> Date: Fri, 17 Jan 2020 19:44:54 -0700 Subject: [PATCH 13/49] Created Velocity Control PID and Interrupt Command --- .../java/frc4388/robot/RobotContainer.java | 7 +++ .../robot/commands/DriveAtVelocityPID.java | 52 +++++++++++++++++++ .../robot/commands/DriveToDistancePID.java | 3 ++ 3 files changed, 62 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fac6bd7..202f647 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -83,6 +84,12 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); + + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + + new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) + .whenPressed(new InstantCommand(null, m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java new file mode 100644 index 0000000..70e54dd --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class DriveAtVelocityPID extends CommandBase { + Drive m_drive; + double m_targetVel; + double m_leftTarget; + double m_rightTarget; + /** + * Creates a new DriveAtVelocityPID. + */ + public DriveAtVelocityPID(Drive subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_targetVel = targetVel; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + 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); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 6c8e1cc..3fe0ea0 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; @@ -31,6 +32,8 @@ public class DriveToDistancePID extends CommandBase { public void initialize() { m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); + SmartDashboard.putNumber("Left Target", m_leftTarget); + SmartDashboard.putNumber("Right Target", m_rightTarget); } // Called every time the scheduler runs while the command is scheduled. From c72a32efb2183ffbccee4479b58ff40728ccde2a Mon Sep 17 00:00:00 2001 From: aarav18 <57469559+aarav18@users.noreply.github.com> Date: Sat, 18 Jan 2020 09:27:53 -0800 Subject: [PATCH 14/49] Fixed Deadband Issue Made motors running at same speed. --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Drive.java | 9 ++++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 202f647..8f58f44 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -88,8 +88,8 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); - new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(null, m_robotDrive)); + //new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) + //.whenPressed(new InstantCommand(null, m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2ad83d9..24fe71c 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -61,6 +61,9 @@ public class Drive extends SubsystemBase { m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE + m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed + /* Motor Encoders */ //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); @@ -83,7 +86,7 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); - + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); @@ -142,8 +145,8 @@ public class Drive extends SubsystemBase { public void setDriveTrainNeutralMode(NeutralMode mode) { m_leftFrontMotor.setNeutralMode(mode); m_rightFrontMotor.setNeutralMode(mode); - m_leftFrontMotor.setNeutralMode(mode); - m_rightFrontMotor.setNeutralMode(mode); + m_leftBackMotor.setNeutralMode(mode); + m_rightBackMotor.setNeutralMode(mode); } /** From 162bbac7bcd317e34ede70bf08cac8cea94a23da Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 12:47:49 -0700 Subject: [PATCH 15/49] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8f58f44..73937ee 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -80,7 +80,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); + .whenPressed(new DriveToDistancePID(m_robotDrive, 40000)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); @@ -88,8 +88,8 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); - //new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - //.whenPressed(new InstantCommand(null, m_robotDrive)); + new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) + .whenPressed(new InstantCommand(() -> System.err.print("interrupt"), m_robotDrive)); } /** From cbc112982cc09613242d703d9b95b0acc0de5b76 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 15:12:48 -0700 Subject: [PATCH 16/49] Added inches to encoder ticks conversion --- src/main/java/frc4388/robot/Constants.java | 15 ++++++++++++++- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- .../robot/commands/DriveAtVelocityPID.java | 5 ++++- .../frc4388/robot/commands/DriveToDistanceMM.java | 5 ++++- .../robot/commands/DriveToDistancePID.java | 11 ++++++++--- 5 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 466ff74..0558df5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -19,6 +19,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class DriveConstants { + /* Drive Train IDs */ public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; public static final int DRIVE_LEFT_BACK_CAN_ID = 3; @@ -31,7 +32,19 @@ public final class Constants { public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); - public static final double ENCODER_TICKS_PER_REV = 2048; + /* Drive Train Characteristics */ + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5; + public static final double WHEEL_DIAMETER_INCHES = 6; + + /* Ratio Calculation */ + public static final double TICK_TIME_TO_SECONDS = 0.1; + public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS; + public static final double WHEEL_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 73937ee..daa3e79 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -80,13 +80,13 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveToDistancePID(m_robotDrive, 40000)); + .whenPressed(new DriveToDistancePID(m_robotDrive, 36)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); + .whenPressed(new DriveToDistanceMM(m_robotDrive, 36)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 12)); 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 70e54dd..91e6a9b 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; public class DriveAtVelocityPID extends CommandBase { @@ -17,11 +18,13 @@ public class DriveAtVelocityPID extends CommandBase { double m_rightTarget; /** * Creates a new DriveAtVelocityPID. + * @param subsystem drive subsystem + * @param distance target velocity in inches/second */ public DriveAtVelocityPID(Drive subsystem, double targetVel) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetVel = targetVel; + m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME; addRequirements(m_drive); } diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java index 0faf431..872a0ce 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; public class DriveToDistanceMM extends CommandBase { @@ -18,11 +19,13 @@ public class DriveToDistanceMM extends CommandBase { /** * Creates a new DriveToDistancePID. + * @param subsystem drive subsystem + * @param distance distance to travel in inches */ public DriveToDistanceMM(Drive subsystem, double distance) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_distance = distance; + m_distance = distance * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); } diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 3fe0ea0..ab46b5d 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -9,6 +9,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; public class DriveToDistancePID extends CommandBase { @@ -19,12 +20,15 @@ public class DriveToDistancePID extends CommandBase { /** * Creates a new DriveToDistancePID. + * @param subsystem drive subsystem + * @param distance distance to travel in inches */ public DriveToDistancePID(Drive subsystem, double distance) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_distance = distance; + m_distance = distance * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); + SmartDashboard.putNumber("Distance Target Inches", distance); } // Called when the command is initially scheduled. @@ -32,8 +36,9 @@ public class DriveToDistancePID extends CommandBase { public void initialize() { m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - SmartDashboard.putNumber("Left Target", m_leftTarget); - SmartDashboard.putNumber("Right Target", m_rightTarget); + SmartDashboard.putNumber("Distance Target Ticks", m_distance); + SmartDashboard.putNumber("Left Target Ticks", m_leftTarget); + SmartDashboard.putNumber("Right Target Ticks", m_rightTarget); } // Called every time the scheduler runs while the command is scheduled. From d525141e355bf467f6e224b41b602e6e184aa00b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 15:18:58 -0700 Subject: [PATCH 17/49] Remove duplicate deadband code --- src/main/java/frc4388/robot/subsystems/Drive.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index eb5f2e7..c168c9d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -53,10 +53,7 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Brake); - - m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE - m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed + setDriveTrainNeutralMode(NeutralMode.Brake); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); From 4568e31036cd97b8a0fbcf75284094060e001bc2 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 15:27:33 -0700 Subject: [PATCH 18/49] Change Teleop Neutral Mode to Brake --- src/main/java/frc4388/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 41328b4..8c15e6a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -97,7 +97,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to From 41ab970692564f13f18231fa62c2a1c02276baf5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 16:12:56 -0700 Subject: [PATCH 19/49] Do Cleanup, Constants, and Docs --- src/main/java/frc4388/robot/Constants.java | 18 ++++++- .../java/frc4388/robot/subsystems/Drive.java | 51 ++++++------------- 2 files changed, 31 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0558df5..4cf913b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -27,10 +27,24 @@ public final class Constants { public static final int PIGEON_ID = 6; /* PID Constants Drive*/ - public static final int DRIVE_SLOT_IDX = 0; - public static final int DRIVE_PID_LOOP_IDX = 0; public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY = 15000; + public static final int DRIVE_ACCELERATION = 6000; + + /* Remote Sensors */ + public final static int REMOTE_0 = 0; + public final static int REMOTE_1 = 1; + + /* PID Indexes */ + public final static int PID_PRIMARY = 0; + public final static int PID_TURN = 1; + + /* PID SLOTS */ + public final static int SLOT_DISTANCE = 0; + public final static int SLOT_VELOCITY = 1; + public final static int SLOT_TURNING = 2; + public final static int SLOT_MOTION_MAGIC = 3; /* Drive Train Characteristics */ public static final double TICKS_PER_MOTOR_REV = 2048; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index c168c9d..7d007b5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,8 +7,10 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; @@ -61,34 +63,14 @@ public class Drive extends SubsystemBase { m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); + /* deadbands */ m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed - /* Motor Encoders */ - //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - - /*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/ - - setDriveTrainGains(); - - m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); - - m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); + setDriveTrainGains(); + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); /* Smart Dashboard Initial Values */ SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); @@ -108,8 +90,6 @@ public class Drive extends SubsystemBase { int closedLoopTimeMs = 1; m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - - } @Override @@ -153,17 +133,16 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void setDriveTrainGains(){ - m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + /* Distance */ + m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_leftFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + /* Motion Magic */ + m_leftFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); } /** From f5fa226d98591a74f45d40c462227adcf50a1eac Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 20 Jan 2020 10:40:12 -0700 Subject: [PATCH 20/49] Set up Talon PIDs --- .../java/frc4388/robot/subsystems/Drive.java | 60 ++++++++++++++++++- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 7d007b5..808cf60 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -10,7 +10,9 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.FeedbackDevice; 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.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; @@ -55,19 +57,58 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Brake); + setDriveTrainNeutralMode(NeutralMode.Brake); + + /* Setup Sensors for TalonFXs */ + + /* Configure the left Talon's selected sensor as local QuadEncoder */ + m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + + /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ + m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source + RemoteSensorSource.TalonSRX_SelectedSensor, // Remote Feedback Source + DriveConstants.REMOTE_0, // Source number [0, 1] + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + + /* 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); + + /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, + DriveConstants.PID_PRIMARY, + DriveConstants.DRIVE_TIMEOUT_MS); + + /* Scale Feedback by 0.5 to half the sum of Distance */ + m_rightFrontMotor.configSelectedFeedbackCoefficient( 0.5, // 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, + 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 */ + 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); /* deadbands */ m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed - setDriveTrainGains(); m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -87,9 +128,24 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + /** + * 1ms per loop. PID loop can be slowed down if need be. + * For example, + * - if sensor updates are too slow + * - sensor deltas are very small per update, so derivative error never gets large enough to be useful. + * - sensor movement is very slow causing the derivative error to be near zero. + */ int closedLoopTimeMs = 1; m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); m_leftFrontMotor.configClosedLoopPeriod(1, 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); } @Override From ab1a670050e1655cd1e7accf1763f61c39af07f1 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 20 Jan 2020 10:46:07 -0700 Subject: [PATCH 21/49] Remove magic numbers --- src/main/java/frc4388/robot/subsystems/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 808cf60..4068296 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -137,8 +137,8 @@ public class Drive extends SubsystemBase { * - sensor movement is very slow causing the derivative error to be near zero. */ int closedLoopTimeMs = 1; - m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); /** * configAuxPIDPolarity(boolean invert, int timeoutMs) From 2896b9fec90140dffb5fae42160ddaf85f88bc25 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 20 Jan 2020 09:55:14 -0800 Subject: [PATCH 22/49] Added more gains --- src/main/java/frc4388/robot/Constants.java | 5 +- .../java/frc4388/robot/subsystems/Drive.java | 86 +++++++++++++++---- 2 files changed, 74 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4cf913b..efcec02 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -28,7 +28,10 @@ public final class Constants { /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_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.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 int DRIVE_CRUISE_VELOCITY = 15000; public static final int DRIVE_ACCELERATION = 6000; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 7d007b5..5e8d380 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -36,7 +36,11 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - public static Gains m_gains = DriveConstants.DRIVE_GAINS; + 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; + /** * Add your docs here. @@ -82,10 +86,25 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); - SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + SmartDashboard.putNumber("P Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kP); + SmartDashboard.putNumber("I Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kI); + SmartDashboard.putNumber("D Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kD); + SmartDashboard.putNumber("F Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kF); + + SmartDashboard.putNumber("P Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kP); + SmartDashboard.putNumber("I Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kI); + SmartDashboard.putNumber("D Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kD); + SmartDashboard.putNumber("F Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kF); + + SmartDashboard.putNumber("P Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kP); + SmartDashboard.putNumber("I Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kI); + SmartDashboard.putNumber("D Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kD); + SmartDashboard.putNumber("F Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kF); + + SmartDashboard.putNumber("P Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kP); + SmartDashboard.putNumber("I Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kI); + SmartDashboard.putNumber("D Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kD); + SmartDashboard.putNumber("F Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kF); int closedLoopTimeMs = 1; m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); @@ -104,10 +123,25 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - m_gains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - m_gains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - m_gains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - m_gains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + m_gainsDistance.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kP); + m_gainsDistance.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kI); + m_gainsDistance.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kD); + m_gainsDistance.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kF); + + m_gainsVelocity.kP = SmartDashboard.getNumber("P Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kP); + m_gainsVelocity.kI = SmartDashboard.getNumber("I Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kI); + m_gainsVelocity.kD = SmartDashboard.getNumber("D Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kD); + m_gainsVelocity.kF = SmartDashboard.getNumber("F Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kF); + + m_gainsTurning.kP = SmartDashboard.getNumber("P Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kP); + m_gainsTurning.kI = SmartDashboard.getNumber("I Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kI); + m_gainsTurning.kD = SmartDashboard.getNumber("D Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kD); + m_gainsTurning.kF = SmartDashboard.getNumber("F Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kF); + + m_gainsMotionMagic.kP = SmartDashboard.getNumber("P Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kP); + m_gainsMotionMagic.kI = SmartDashboard.getNumber("I Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kI); + m_gainsMotionMagic.kD = SmartDashboard.getNumber("D Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kD); + m_gainsMotionMagic.kF = SmartDashboard.getNumber("F Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kF); setDriveTrainGains(); @@ -134,15 +168,35 @@ public class Drive extends SubsystemBase { */ public void setDriveTrainGains(){ /* Distance */ - m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_leftFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kD, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Velocity */ + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Turning */ + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS); /* Motion Magic */ - m_leftFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kD, 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); } /** From 2d1ba14cfd106c7a6b6931f60bfb9c596f2fb007 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 20 Jan 2020 11:59:23 -0700 Subject: [PATCH 23/49] Create a chooser to switch gains --- .../java/frc4388/robot/subsystems/Drive.java | 137 +++++++++--------- 1 file changed, 69 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index a2f06f2..6d5141e 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -18,6 +18,8 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; @@ -38,12 +40,12 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + SendableChooser m_chooser = new SendableChooser(); 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; - /** * Add your docs here. */ @@ -113,40 +115,39 @@ public class Drive extends SubsystemBase { m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed - setDriveTrainGains(); + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); /* Smart Dashboard Initial Values */ + /* Set up Chooser */ + m_chooser.setDefaultOption("Distance PID", m_gainsDistance); + setDriveTrainGains("Distance PID", m_gainsDistance); + m_chooser.addOption("Velocity PID", m_gainsVelocity); + setDriveTrainGains("Velocity PID", m_gainsVelocity); + m_chooser.addOption("Turning PID", m_gainsTurning); + setDriveTrainGains("Turning PID", m_gainsTurning); + m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic); + setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic); + Shuffleboard.getTab("PID").add(m_chooser); + + /* Gyro */ SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + /* Sensor Values */ SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); - SmartDashboard.putNumber("P Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kP); - SmartDashboard.putNumber("I Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kI); - SmartDashboard.putNumber("D Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kD); - SmartDashboard.putNumber("F Value Drive Distance", DriveConstants.DRIVE_DISTANCE_GAINS.kF); - - SmartDashboard.putNumber("P Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kP); - SmartDashboard.putNumber("I Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kI); - SmartDashboard.putNumber("D Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kD); - SmartDashboard.putNumber("F Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kF); - - SmartDashboard.putNumber("P Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kP); - SmartDashboard.putNumber("I Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kI); - SmartDashboard.putNumber("D Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kD); - SmartDashboard.putNumber("F Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kF); - - SmartDashboard.putNumber("P Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kP); - SmartDashboard.putNumber("I Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kI); - SmartDashboard.putNumber("D Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kD); - SmartDashboard.putNumber("F Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kF); - + /* PID */ + Gains gains = m_chooser.getSelected(); + SmartDashboard.putNumber("P Value Drive", gains.kP); + SmartDashboard.putNumber("I Value Drive", gains.kI); + SmartDashboard.putNumber("D Value Drive", gains.kD); + SmartDashboard.putNumber("F Value Drive", gains.kF); /** * 1ms per loop. PID loop can be slowed down if need be. @@ -167,6 +168,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS); } + Gains gainsOld; @Override public void periodic() { try { @@ -179,27 +181,19 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - m_gainsDistance.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kP); - m_gainsDistance.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kI); - m_gainsDistance.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kD); - m_gainsDistance.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_DISTANCE_GAINS.kF); - - m_gainsVelocity.kP = SmartDashboard.getNumber("P Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kP); - m_gainsVelocity.kI = SmartDashboard.getNumber("I Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kI); - m_gainsVelocity.kD = SmartDashboard.getNumber("D Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kD); - m_gainsVelocity.kF = SmartDashboard.getNumber("F Value Drive Velocity", DriveConstants.DRIVE_VELOCITY_GAINS.kF); - - m_gainsTurning.kP = SmartDashboard.getNumber("P Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kP); - m_gainsTurning.kI = SmartDashboard.getNumber("I Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kI); - m_gainsTurning.kD = SmartDashboard.getNumber("D Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kD); - m_gainsTurning.kF = SmartDashboard.getNumber("F Value Drive Turning", DriveConstants.DRIVE_TURNING_GAINS.kF); - - m_gainsMotionMagic.kP = SmartDashboard.getNumber("P Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kP); - m_gainsMotionMagic.kI = SmartDashboard.getNumber("I Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kI); - m_gainsMotionMagic.kD = SmartDashboard.getNumber("D Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kD); - m_gainsMotionMagic.kF = SmartDashboard.getNumber("F Value Drive Motion Magic", DriveConstants.DRIVE_MOTION_MAGIC_GAINS.kF); - - setDriveTrainGains(); + Gains gains = m_chooser.getSelected(); + if (gains.equals(gainsOld)) { + SmartDashboard.putNumber("P Value Drive", gains.kP); + SmartDashboard.putNumber("I Value Drive", gains.kI); + SmartDashboard.putNumber("D Value Drive", gains.kD); + SmartDashboard.putNumber("F Value Drive", gains.kF); + } + gains.kP = SmartDashboard.getNumber("P Value Drive", gains.kP); + gains.kI = SmartDashboard.getNumber("I Value Drive", gains.kI); + gains.kD = SmartDashboard.getNumber("D Value Drive", gains.kD); + gains.kF = SmartDashboard.getNumber("F Value Drive", gains.kF); + setDriveTrainGains(m_chooser.getName(), gains); + gainsOld = gains; } catch (Exception e) { @@ -222,37 +216,44 @@ public class Drive extends SubsystemBase { /** * Add your docs here. */ - public void setDriveTrainGains(){ + public void setDriveTrainGains(String slot, Gains gains){ /* Distance */ - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kD, DriveConstants.DRIVE_TIMEOUT_MS); + if (slot.equals("Distance PID")) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + } /* Velocity */ - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); - + if (slot.equals("Velocity PID")) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + } /* Turning */ - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS); + if (slot.equals("Turning PID")) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + } /* Motion Magic */ - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.kD, 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); + if (slot.equals("Motion Magic PID")) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.kD, 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); + } } /** From 7008ceb4539bfda74b1e5323331386a098c71f9a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 23 Jan 2020 16:42:20 -0700 Subject: [PATCH 24/49] 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); From 0da2e3540072f55168017898533037acd24045a0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 24 Jan 2020 17:20:15 -0700 Subject: [PATCH 25/49] Uncommented Velocity PID --- .../java/frc4388/robot/commands/DriveAtVelocityPID.java | 6 +++--- src/main/java/frc4388/robot/subsystems/Drive.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index c7fa702..229a7b6 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -40,9 +40,9 @@ public class DriveAtVelocityPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //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); + m_drive.runVelocityPID(m_rightTarget); + m_drive.runVelocityPID(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 99e1c4e..21b3c9f 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -311,7 +311,7 @@ public class Drive extends SubsystemBase { 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); + //m_rightFrontMotor.set(DemandType.AuxPID, 0); } public double getGyroYaw() { From dec773cdcdf9349821993a1b0f2e8831d5ab2a50 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 24 Jan 2020 20:27:42 -0700 Subject: [PATCH 26/49] Velocity Control PID Does not work in testing --- .../java/frc4388/robot/RobotContainer.java | 4 +- .../robot/commands/DriveAtVelocityPID.java | 16 +++-- .../java/frc4388/robot/subsystems/Drive.java | 64 ++++++++++--------- 3 files changed, 48 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3d6ad6a..b362cd8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); + //m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on @@ -79,7 +79,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.runVelocityPID(2000), m_robotDrive)); + .whileHeld(new DriveAtVelocityPID(m_robotDrive, 50)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index 229a7b6..f8c8669 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -9,6 +9,7 @@ package frc4388.robot.commands; import com.ctre.phoenix.motorcontrol.FollowerType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; @@ -18,6 +19,7 @@ public class DriveAtVelocityPID extends CommandBase { double m_targetVel; double m_leftTarget; double m_rightTarget; + double m_copiedTargetVel; /** * Creates a new DriveAtVelocityPID. * @param subsystem drive subsystem @@ -26,23 +28,27 @@ 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; + m_copiedTargetVel = targetVel; addRequirements(m_drive); } // Called when the command is initially scheduled. @Override public void initialize() { - m_leftTarget = -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_rightTarget); - m_drive.runVelocityPID(m_leftTarget); - m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); + m_drive.runVelocityPID(m_targetVel); + + SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); + SmartDashboard.putNumber("Output Target Velocity", m_targetVel); + //m_drive.runVelocityPID(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 21b3c9f..097f9ed 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -75,42 +75,45 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ - m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source + /*m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source RemoteSensorSource.TalonSRX_SelectedSensor, // Remote Feedback Source DriveConstants.REMOTE_0, // Source number [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + 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(), + /*m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw, DriveConstants.REMOTE_1, - DriveConstants.DRIVE_TIMEOUT_MS); + 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); + //m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, - DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, + // DriveConstants.PID_PRIMARY, + // DriveConstants.DRIVE_TIMEOUT_MS); /* Scale Feedback by 0.5 to half the sum of Distance */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient - DriveConstants.PID_PRIMARY, // PID Slot of Source - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + //m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient + // DriveConstants.PID_PRIMARY, // PID Slot of Source + // DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, - DriveConstants.PID_TURN, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, + // DriveConstants.PID_TURN, + // DriveConstants.DRIVE_TIMEOUT_MS); /* Scale the Feedback Sensor using a coefficient */ - m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, - DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, + // DriveConstants.PID_PRIMARY, + // DriveConstants.DRIVE_TIMEOUT_MS); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); @@ -123,10 +126,10 @@ public class Drive extends SubsystemBase { 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_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_rightFrontMotor.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); /* deadbands */ @@ -170,10 +173,10 @@ public class Drive extends SubsystemBase { * 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); + m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); /** * 1ms per loop. PID loop can be slowed down if need be. @@ -184,7 +187,7 @@ public class Drive extends SubsystemBase { */ int closedLoopTimeMs = 1; m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); /** * configAuxPIDPolarity(boolean invert, int timeoutMs) @@ -296,9 +299,12 @@ public class Drive extends SubsystemBase { 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.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVelLeft, DemandType.AuxPID, 0); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); + //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); } public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){ From 9b6c6ec6583cc127f63b96f4e48966c56d3cdf1e Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 27 Jan 2020 19:06:23 -0700 Subject: [PATCH 27/49] work done on PID in better state than last time but still bad --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 4 +- .../robot/commands/DriveAtVelocityPID.java | 6 +- .../java/frc4388/robot/subsystems/Drive.java | 91 ++++++++++++------- 4 files changed, 67 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f7d5130..7608c30 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,8 +29,8 @@ 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, 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_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.3); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.01, 0.0, 0.0, 0.0, 0, 0.3); 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 b362cd8..5d2b182 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - //m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on @@ -79,7 +79,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new DriveAtVelocityPID(m_robotDrive, 50)); + .whileHeld(new DriveAtVelocityPID(m_robotDrive, 20)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index f8c8669..a1e7501 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -16,6 +16,7 @@ import frc4388.robot.subsystems.Drive; public class DriveAtVelocityPID extends CommandBase { Drive m_drive; + double m_targetGyro; double m_targetVel; double m_leftTarget; double m_rightTarget; @@ -38,15 +39,18 @@ public class DriveAtVelocityPID extends CommandBase { public void initialize() { m_leftTarget = m_targetVel; m_rightTarget = m_targetVel; + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runVelocityPID(m_targetVel); + m_drive.runVelocityPID(m_targetVel, 0); SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); SmartDashboard.putNumber("Output Target Velocity", m_targetVel); + + SmartDashboard.putNumber("Target Angle", m_targetGyro); //m_drive.runVelocityPID(m_leftTarget); //m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 097f9ed..6fcf8b3 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -70,45 +70,58 @@ public class Drive extends SubsystemBase { setDriveTrainNeutralMode(NeutralMode.Brake); /* Setup Sensors for TalonFXs */ + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure the left Talon's selected sensor as local QuadEncoder */ m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ - /*m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source - RemoteSensorSource.TalonSRX_SelectedSensor, // Remote Feedback Source + m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source + RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); */ // Configuration Timeout + 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(), + m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw, DriveConstants.REMOTE_1, - DriveConstants.DRIVE_TIMEOUT_MS);*/ + 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); + m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - //m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, - // DriveConstants.PID_PRIMARY, - // DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, + DriveConstants.PID_PRIMARY, + DriveConstants.DRIVE_TIMEOUT_MS); /* Scale Feedback by 0.5 to half the sum of Distance */ - //m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient - // DriveConstants.PID_PRIMARY, // PID Slot of Source - // DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + m_rightFrontMotor.configSelectedFeedbackCoefficient( 0.5, // Coefficient + DriveConstants.PID_PRIMARY, // PID Slot of Source + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, + DriveConstants.PID_TURN, + DriveConstants.DRIVE_TIMEOUT_MS); + + /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, + DriveConstants.PID_TURN, + DriveConstants.DRIVE_TIMEOUT_MS);*/ /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ - //m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, - // DriveConstants.PID_TURN, - // DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, + DriveConstants.PID_TURN, + DriveConstants.DRIVE_TIMEOUT_MS); /* Scale the Feedback Sensor using a coefficient */ //m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, @@ -120,24 +133,21 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.setSensorPhase(true); + m_rightFrontMotor.setSensorPhase(false); 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_rightFrontMotor.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); + /* 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_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); /* deadbands */ 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); /* Smart Dashboard Initial Values */ /* Set up Chooser */ @@ -187,14 +197,14 @@ public class Drive extends SubsystemBase { */ int closedLoopTimeMs = 1; m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, 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(true, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS); } Gains gainsOld; @@ -210,6 +220,15 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Output", m_rightFrontMotor.getMotorOutputPercent()); + SmartDashboard.putNumber("Left Motor Output", m_leftFrontMotor.getMotorOutputPercent()); + SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); + SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); + SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); + Gains gains = m_chooser.getSelected(); if (gains.equals(gainsOld)) { SmartDashboard.putNumber("P Value Drive", gains.kP); @@ -262,6 +281,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); } /* Turning */ if (slot.equals("Turning PID")) { @@ -270,6 +290,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); } /* Motion Magic */ @@ -289,6 +310,7 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void driveWithInput(double move, double steer){ + m_leftFrontMotor.setInverted(false); m_driveTrain.arcadeDrive(move, steer); } @@ -297,13 +319,14 @@ public class Drive extends SubsystemBase { talon.set(TalonFXControlMode.Position, targetPos); } - public void runVelocityPID(double targetVel) { + public void runVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); //m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVelLeft, DemandType.AuxPID, 0); + //m_leftFrontMotor.setInverted(true); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); } @@ -324,6 +347,8 @@ public class Drive extends SubsystemBase { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); return ypr[0]; + //m_pigeon.(ypr); + //return ypr[0]; } public double getGyroPitch() { From 29593564c2bd14a63f2b88bf5db178d29403eb72 Mon Sep 17 00:00:00 2001 From: RishabhCowlagi <59751356+RishabhCowlagi@users.noreply.github.com> Date: Tue, 28 Jan 2020 17:07:49 -0800 Subject: [PATCH 28/49] Add Basic Storage Code --- src/main/java/frc4388/robot/Constants.java | 10 ++- .../frc4388/robot/subsystems/Storage.java | 48 +++++++++++++ vendordeps/REVRobotics.json | 70 +++++++++++++++++++ 3 files changed, 127 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Storage.java create mode 100644 vendordeps/REVRobotics.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 793ce5c..eb408fa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,7 +29,15 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 1; } - + public static final class StorageConstants { + public static final int STORAGE_TALON_ID = 9; + public static final int BEAM_SENSOR_DIO_0 = 0; + public static final int BEAM_SENSOR_DIO_1 = 1; + public static final int BEAM_SENSOR_DIO_2 = 2; + public static final int BEAM_SENSOR_DIO_3 = 3; + public static final int BEAM_SENSOR_DIO_4 = 0; + public static final int BEAM_SENSOR_DIO_5 = 0; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java new file mode 100644 index 0000000..7b85a0d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.StorageConstants; + +public class Storage extends SubsystemBase { + private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_TALON_ID, MotorType.kBrushless); + private DigitalInput[] m_beamSensors = new DigitalInput[6]; + /** + * Creates a new Storage. + */ + public Storage() { + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + } + + @Override + public void periodic() { + // NO + } + + /** + * Runs storage motor + * @param input the voltage to run motor at + */ + public void runStorage(double input) { + m_storageMotor.set(input); + boolean beam_on = m_beamSensors[0].get(); + } +} diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json new file mode 100644 index 0000000..63380b6 --- /dev/null +++ b/vendordeps/REVRobotics.json @@ -0,0 +1,70 @@ +{ + "fileName": "REVRobotics.json", + "name": "REVRobotics", + "version": "1.5.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-java", + "version": "1.5.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-cpp", + "version": "1.5.1", + "libName": "SparkMax", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "libName": "SparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file From e96ccc527af57e008deeabc2b736d6be955b0ea5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 16:44:05 -0700 Subject: [PATCH 29/49] more work done (getting closer) --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 6 ++--- .../robot/commands/DriveAtVelocityPID.java | 5 +++- .../java/frc4388/robot/subsystems/Drive.java | 27 +++++++------------ 5 files changed, 19 insertions(+), 23 deletions(-) diff --git a/build.gradle b/build.gradle index e00a044..e0194f3 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.1.2" + id "edu.wpi.first.GradleRIO" version "2020.2.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7608c30..4c96e86 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,7 +30,7 @@ public final class Constants { 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, 1.0); public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.3); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.01, 0.0, 0.0, 0.0, 0, 0.3); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3); 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 5d2b182..ba1ef76 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,16 +79,16 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new DriveAtVelocityPID(m_robotDrive, 20)); + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 0)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive)); + .whileHeld(new RunCommand(() -> m_robotDrive.driveWithInput(0, 0.3), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> System.err.print("interrupt"), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.getCurrentCommand().cancel(), m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index a1e7501..68fd295 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import com.ctre.phoenix.motorcontrol.FollowerType; +import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -40,12 +41,13 @@ public class DriveAtVelocityPID extends CommandBase { m_leftTarget = m_targetVel; m_rightTarget = m_targetVel; m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runVelocityPID(m_targetVel, 0); + m_drive.runVelocityPID(m_targetVel, m_targetGyro+1000); SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); SmartDashboard.putNumber("Output Target Velocity", m_targetVel); @@ -58,6 +60,7 @@ public class DriveAtVelocityPID extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_drive.setDriveTrainNeutralMode(NeutralMode.Brake); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 6fcf8b3..272bc54 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -98,19 +98,19 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, DriveConstants.PID_PRIMARY, 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 m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); @@ -133,10 +133,6 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.setSensorPhase(false); - 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); @@ -174,10 +170,10 @@ public class Drive extends SubsystemBase { /* PID */ Gains gains = m_chooser.getSelected(); - SmartDashboard.putNumber("P Value Drive", gains.kP); - SmartDashboard.putNumber("I Value Drive", gains.kI); - SmartDashboard.putNumber("D Value Drive", gains.kD); - SmartDashboard.putNumber("F Value Drive", gains.kF); + Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); + Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); + Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); + Shuffleboard.getTab("PID").add("F Value Drive", gains.kF); /** * Max out the peak output (for all modes). @@ -310,7 +306,7 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void driveWithInput(double move, double steer){ - m_leftFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(false); m_driveTrain.arcadeDrive(move, steer); } @@ -321,13 +317,10 @@ public class Drive extends SubsystemBase { public void runVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - //m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVelLeft, DemandType.AuxPID, 0); - //m_leftFrontMotor.setInverted(true); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); + m_rightFrontMotor.setInverted(false); + m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, targetVel, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); } public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){ From ae5fc4cb00dee49b3aadedaaf8d4581958fbc52d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 21:36:11 -0700 Subject: [PATCH 30/49] Invert Right side so PID Loops work - Invert Right Motor - Invert Right side on differential drive to allow for driving normally without subsequent motor inverts --- src/main/java/frc4388/robot/subsystems/Drive.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 272bc54..8c9e346 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -130,7 +130,8 @@ public class Drive extends SubsystemBase { /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(true); + m_driveTrain.setRightSideInverted(true); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); @@ -306,7 +307,7 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void driveWithInput(double move, double steer){ - m_rightFrontMotor.setInverted(false); + //m_rightFrontMotor.setInverted(false); m_driveTrain.arcadeDrive(move, steer); } @@ -318,7 +319,7 @@ public class Drive extends SubsystemBase { public void runVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.setInverted(false); + //m_rightFrontMotor.setInverted(false); m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, targetVel, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); } From 5cb458082a36c142bb7c21d4c3d0fddcfe7d4a8a Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 21:46:56 -0700 Subject: [PATCH 31/49] Modify Turning PID - Uses a modified Velocity PID to keep robot still while rotating - Could also use a Position PID to keep robot still but should be looked into further --- src/main/java/frc4388/robot/Constants.java | 1 + src/main/java/frc4388/robot/subsystems/Drive.java | 11 ++++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4c96e86..f4f8574 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -53,6 +53,7 @@ public final class Constants { public static final double TICKS_PER_MOTOR_REV = 2048; public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5; public static final double WHEEL_DIAMETER_INCHES = 6; + public static final double TICKS_PER_GYRO_REV = 8192; /* Ratio Calculation */ public static final double TICK_TIME_TO_SECONDS = 0.1; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 8c9e346..e1dd0ee 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -329,12 +329,13 @@ public class Drive extends SubsystemBase { talon.set(TalonFXControlMode.MotionMagic, targetPos); } + /** + * Runs a Turning PID to rotate a to a target angle + * @param targetAngle target angle in degrees + */ 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); + double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV; + runVelocityPID(0, targetGyro); } public double getGyroYaw() { From 009443b0446a594674d7af2e39b5d1477c321979 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 23:21:35 -0700 Subject: [PATCH 32/49] Minor Code Cleanup --- src/main/java/frc4388/robot/subsystems/Drive.java | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index e1dd0ee..e4f61e1 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -77,9 +77,10 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + + /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout + DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source @@ -113,10 +114,6 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - - /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, - DriveConstants.PID_TURN, - DriveConstants.DRIVE_TIMEOUT_MS);*/ /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, @@ -342,8 +339,6 @@ public class Drive extends SubsystemBase { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); return ypr[0]; - //m_pigeon.(ypr); - //return ypr[0]; } public double getGyroPitch() { From 2d14ef3da30a0d613e3db37dfa7e5b1887695e1f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 31 Jan 2020 17:50:43 -0700 Subject: [PATCH 33/49] Araav Stuff for PID --- src/main/java/frc4388/robot/Constants.java | 10 ++--- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 7 ++-- .../robot/commands/DriveAtVelocityPID.java | 5 ++- .../robot/commands/DriveToDistanceMM.java | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 39 ++++++++++++------- 6 files changed, 39 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4c96e86..1627a0a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -28,12 +28,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, 1.0); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.3); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3); + 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.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.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; + public static final int DRIVE_CRUISE_VELOCITY = 2000; + public static final int DRIVE_ACCELERATION = 1000; /* Remote Sensors */ public final static int REMOTE_0 = 0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 8c15e6a..41328b4 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -97,7 +97,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); + m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ba1ef76..ed87459 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,16 +79,17 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveAtVelocityPID(m_robotDrive, 0)); + .whenPressed(new DriveAtVelocityPID(m_robotDrive, 150)) + .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.driveWithInput(0, 0.3), m_robotDrive)); + .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.getCurrentCommand().cancel(), m_robotDrive)); + .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index 68fd295..7d241b6 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -42,12 +42,13 @@ public class DriveAtVelocityPID extends CommandBase { m_rightTarget = m_targetVel; m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runVelocityPID(m_targetVel, m_targetGyro+1000); + m_drive.runVelocityPID(-20000, m_targetGyro); SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); SmartDashboard.putNumber("Output Target Velocity", m_targetVel); @@ -60,7 +61,7 @@ public class DriveAtVelocityPID extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drive.setDriveTrainNeutralMode(NeutralMode.Brake); + m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java index 872a0ce..625e522 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -39,8 +39,8 @@ public class DriveToDistanceMM extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); + //m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); + //m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); } // 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 272bc54..489f97a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.FollowerType; @@ -67,7 +68,7 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Brake); + setDriveTrainNeutralMode(NeutralMode.Coast); /* Setup Sensors for TalonFXs */ m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -216,14 +217,17 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Motor Output", m_rightFrontMotor.getMotorOutputPercent()); - SmartDashboard.putNumber("Left Motor Output", m_leftFrontMotor.getMotorOutputPercent()); - SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); - SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent()); + + ////SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); Gains gains = m_chooser.getSelected(); if (gains.equals(gainsOld)) { @@ -318,14 +322,19 @@ public class Drive extends SubsystemBase { public void runVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.setInverted(false); - m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, targetVel, DemandType.AuxPID, targetGyro); - m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_rightFrontMotor.setInverted(true); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, 20000, DemandType.AuxPID, targetGyro); + //m_rightFrontMotor.set(ControlMode.PercentOutput, 0.2); + //m_leftFrontMotor.follow(m_rightFrontMotor); + //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 runMotionMagicPID(double targetPos){ + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.setInverted(true); + m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); } public void runTurningPID(double targetAngle){ From 46812d16034382dc5ef34981b67541f87b8eda1b Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 19:31:40 -0700 Subject: [PATCH 34/49] Add kF Not under load --- src/main/java/frc4388/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c101572..08a41df 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,7 +29,7 @@ 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.0, 0, 1.0); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.0448767878, 0, 1.0); public static final Gains DRIVE_TURNING_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.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 = 2000; From e9122984d0a221bcda0e52d18f64df4ccdacfc81 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 1 Feb 2020 13:48:52 -0700 Subject: [PATCH 35/49] The giving up commit --- .../robot/commands/DriveToDistancePID.java | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 77 +++++++++++-------- 2 files changed, 49 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index ab46b5d..22f8fa8 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -44,8 +44,8 @@ public class DriveToDistancePID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); + //m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); + //m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); } // 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 ed3261b..b4a733f 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -18,6 +18,7 @@ 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.TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; @@ -37,13 +38,13 @@ public class Drive extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. - public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + public TalonFX m_leftFrontMotor = new TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); + public TalonFX m_rightFrontMotor = new TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); + public TalonFX m_leftBackMotor = new TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); + public TalonFX m_rightBackMotor = new TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + //public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -64,8 +65,21 @@ public class Drive extends SubsystemBase { resetGyroYaw(); /* set back motors as followers */ - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + //m_leftBackMotor.follow(m_leftFrontMotor); + //m_rightBackMotor.follow(m_rightFrontMotor); + + m_rightFrontMotor.setNeutralMode(NeutralMode.Coast); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + } + + void nada(){ + /* set neutral mode */ setDriveTrainNeutralMode(NeutralMode.Coast); @@ -129,7 +143,7 @@ public class Drive extends SubsystemBase { /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(true); - m_driveTrain.setRightSideInverted(true); + //m_driveTrain.setRightSideInverted(true); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); @@ -202,32 +216,32 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS); } - Gains gainsOld; + Gains gainsOld = m_chooser.getSelected(); @Override public void periodic() { try { - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); - SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + //SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + //SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); + //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent()); - ////SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - Gains gains = m_chooser.getSelected(); + /*Gains gains = m_chooser.getSelected(); if (gains.equals(gainsOld)) { SmartDashboard.putNumber("P Value Drive", gains.kP); SmartDashboard.putNumber("I Value Drive", gains.kI); @@ -239,12 +253,11 @@ public class Drive extends SubsystemBase { gains.kD = SmartDashboard.getNumber("D Value Drive", gains.kD); gains.kF = SmartDashboard.getNumber("F Value Drive", gains.kF); setDriveTrainGains(m_chooser.getName(), gains); - gainsOld = gains; + gainsOld = gains;*/ } catch (Exception e) { - - System.err.println("The programming team has failed successfully in the Drive Subsystem."); - + System.err.println("Error in the Drive Subsystem"); + //e.printStackTrace(System.err); } } @@ -309,7 +322,7 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer){ //m_rightFrontMotor.setInverted(false); - m_driveTrain.arcadeDrive(move, steer); + //m_driveTrain.arcadeDrive(move, steer); } public void runPositionPID(WPI_TalonFX talon, double targetPos) { @@ -317,11 +330,15 @@ public class Drive extends SubsystemBase { talon.set(TalonFXControlMode.Position, targetPos); } + long last = 0; public void runVelocityPID(double targetVel, double targetGyro) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); //m_rightFrontMotor.setInverted(true); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, 20000, DemandType.AuxPID, targetGyro); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, 20000);//, DemandType.AuxPID, targetGyro); + long now = System.nanoTime(); + SmartDashboard.putNumber("Loop Time", (now-last)/1000000); + last = now; //m_rightFrontMotor.set(ControlMode.PercentOutput, 0.2); //m_leftFrontMotor.follow(m_rightFrontMotor); //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); From 63fae30f1dc1a7fe59f605e01d99f69d1102e9c9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 1 Feb 2020 15:30:41 -0700 Subject: [PATCH 36/49] PID stuff --- .../java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Drive.java | 36 ++++++++++--------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ed87459..288bdee 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -91,7 +91,7 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); } - + /** * Sets Motors to a NeutralMode. * @param mode NeutralMode to set motors to diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index b4a733f..6ab8d91 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -18,7 +18,6 @@ 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.TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; @@ -38,13 +37,13 @@ public class Drive extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. - public TalonFX m_leftFrontMotor = new TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public TalonFX m_rightFrontMotor = new TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public TalonFX m_leftBackMotor = new TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public TalonFX m_rightBackMotor = new TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); + public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); + public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); + public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); - //public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -65,8 +64,8 @@ public class Drive extends SubsystemBase { resetGyroYaw(); /* set back motors as followers */ - //m_leftBackMotor.follow(m_leftFrontMotor); - //m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); m_rightFrontMotor.setNeutralMode(NeutralMode.Coast); @@ -84,7 +83,7 @@ public class Drive extends SubsystemBase { /* set neutral mode */ setDriveTrainNeutralMode(NeutralMode.Coast); - /* Setup Sensors for TalonFXs */ + /* Setup Sensors for WPI_TalonFXs */ m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -143,9 +142,9 @@ public class Drive extends SubsystemBase { /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(true); - //m_driveTrain.setRightSideInverted(true); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_driveTrain.setRightSideInverted(false); /* Set status frame periods to ensure we don't have stale data */ m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); @@ -322,34 +321,39 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer){ //m_rightFrontMotor.setInverted(false); - //m_driveTrain.arcadeDrive(move, steer); + m_driveTrain.arcadeDrive(move, steer); + + m_driveTrain.feedWatchdog(); } public void runPositionPID(WPI_TalonFX talon, double targetPos) { talon.selectProfileSlot(DriveConstants.SLOT_DISTANCE , DriveConstants.PID_PRIMARY); talon.set(TalonFXControlMode.Position, targetPos); + + m_driveTrain.feedWatchdog(); } long last = 0; public void runVelocityPID(double targetVel, double targetGyro) { //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - //m_rightFrontMotor.setInverted(true); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, 20000);//, DemandType.AuxPID, targetGyro); + m_rightBackMotor.follow(m_rightFrontMotor); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, 1000);//, DemandType.AuxPID, targetGyro); long now = System.nanoTime(); SmartDashboard.putNumber("Loop Time", (now-last)/1000000); last = now; - //m_rightFrontMotor.set(ControlMode.PercentOutput, 0.2); - //m_leftFrontMotor.follow(m_rightFrontMotor); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_driveTrain.feedWatchdog(); } public void runMotionMagicPID(double targetPos){ m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.setInverted(true); m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + + m_driveTrain.feedWatchdog(); } /** From 3a59650611d0f6c4639ccfe675aca9bffa48f940 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 1 Feb 2020 16:10:39 -0700 Subject: [PATCH 37/49] More PID Stuff with Nada Code is working, now we are in the process of slowly adding features from Nada back in. The stuff to add back in is in single line comments, the actual comments are in multi-line comments. --- .../java/frc4388/robot/subsystems/Drive.java | 163 +++++++++--------- 1 file changed, 79 insertions(+), 84 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 6ab8d91..212cff9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -60,68 +60,74 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configFactoryDefault(); m_leftBackMotor.configFactoryDefault(); m_rightBackMotor.configFactoryDefault(); - m_pigeon.configFactoryDefault(); - resetGyroYaw(); + //m_pigeon.configFactoryDefault(); + //resetGyroYaw(); /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); - m_rightFrontMotor.setNeutralMode(NeutralMode.Coast); + setDriveTrainNeutralMode(NeutralMode.Coast); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + /* deadbands */ + 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 + + /* flip input so forward becomes back, etc */ + m_leftFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(true); + m_driveTrain.setRightSideInverted(false); + m_leftBackMotor.setInverted(InvertType.FollowMaster); + m_rightBackMotor.setInverted(InvertType.FollowMaster); + + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - } - - void nada(){ - - - /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Coast); + //Beginning of nada /* Setup Sensors for WPI_TalonFXs */ - m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure the left Talon's selected sensor as local QuadEncoder */ - m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source - DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + //m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + // DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] + // DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ - m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source - RemoteSensorSource.TalonSRX_SelectedSensor, - DriveConstants.REMOTE_0, // Source number [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + //m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source + // RemoteSensorSource.TalonSRX_SelectedSensor, + // 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); + //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); + //m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, - DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, + // DriveConstants.PID_PRIMARY, + // DriveConstants.DRIVE_TIMEOUT_MS); /* Scale Feedback by 0.5 to half the sum of Distance */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient + /*m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient DriveConstants.PID_PRIMARY, // PID Slot of Source DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); @@ -130,71 +136,61 @@ public class Drive extends SubsystemBase { DriveConstants.DRIVE_TIMEOUT_MS); /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, + /*m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); /* 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(true); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_driveTrain.setRightSideInverted(false); - - /* 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_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); - - /* deadbands */ - 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.configSelectedFeedbackCoefficient( 1.0, + DriveConstants.PID_PRIMARY, + DriveConstants.DRIVE_TIMEOUT_MS); + */ + /* 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_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); + /* Smart Dashboard Initial Values */ + /* Set up Chooser */ - m_chooser.setDefaultOption("Distance PID", m_gainsDistance); - setDriveTrainGains("Distance PID", m_gainsDistance); - m_chooser.addOption("Velocity PID", m_gainsVelocity); - setDriveTrainGains("Velocity PID", m_gainsVelocity); - m_chooser.addOption("Turning PID", m_gainsTurning); - setDriveTrainGains("Turning PID", m_gainsTurning); - m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic); - setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic); - Shuffleboard.getTab("PID").add(m_chooser); + //m_chooser.setDefaultOption("Distance PID", m_gainsDistance); + //setDriveTrainGains("Distance PID", m_gainsDistance); + //m_chooser.addOption("Velocity PID", m_gainsVelocity); + //setDriveTrainGains("Velocity PID", m_gainsVelocity); + //m_chooser.addOption("Turning PID", m_gainsTurning); + //setDriveTrainGains("Turning PID", m_gainsTurning); + //m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic); + //setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic); + //Shuffleboard.getTab("PID").add(m_chooser); /* Gyro */ - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); /* Sensor Values */ - SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); - SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); + //SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + //SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); + //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); /* PID */ - Gains gains = m_chooser.getSelected(); - Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); - Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); - Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); - Shuffleboard.getTab("PID").add("F Value Drive", gains.kF); + //Gains gains = m_chooser.getSelected(); + //Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); + //Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); + //Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); + //Shuffleboard.getTab("PID").add("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(+1, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); /** * 1ms per loop. PID loop can be slowed down if need be. @@ -203,16 +199,16 @@ public class Drive extends SubsystemBase { * - sensor deltas are very small per update, so derivative error never gets large enough to be useful. * - sensor movement is very slow causing the derivative error to be near zero. */ - int closedLoopTimeMs = 1; - m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + //int closedLoopTimeMs = 1; + //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(false, DriveConstants.DRIVE_TIMEOUT_MS); } Gains gainsOld = m_chooser.getSelected(); @@ -320,7 +316,6 @@ public class Drive extends SubsystemBase { * Add your docs here. */ public void driveWithInput(double move, double steer){ - //m_rightFrontMotor.setInverted(false); m_driveTrain.arcadeDrive(move, steer); m_driveTrain.feedWatchdog(); From a8a0f60479a183f0994ec63f673d44cae7e8fc55 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 3 Feb 2020 17:31:09 -0700 Subject: [PATCH 38/49] Code cleanup --- src/main/java/frc4388/robot/subsystems/Drive.java | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 212cff9..5381f38 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -81,12 +81,12 @@ public class Drive extends SubsystemBase { m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); //Beginning of nada /* Setup Sensors for WPI_TalonFXs */ @@ -317,8 +317,6 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer){ m_driveTrain.arcadeDrive(move, steer); - - m_driveTrain.feedWatchdog(); } public void runPositionPID(WPI_TalonFX talon, double targetPos) { From 5c7895d2a4da6d845d6342d75661fc5525f3a99f Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 3 Feb 2020 17:31:37 -0700 Subject: [PATCH 39/49] Add Motion Magic Aux PID --- src/main/java/frc4388/robot/subsystems/Drive.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 5381f38..0258286 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -340,12 +340,11 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - public void runMotionMagicPID(double targetPos){ + public void runMotionMagicPID(double targetPos, double targetGyro){ m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos); + m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_driveTrain.feedWatchdog(); } From 4dc07d718a316e370d309a520c2671d1c43b2ddd Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 3 Feb 2020 17:49:05 -0700 Subject: [PATCH 40/49] Fix build error in RobotContainer --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 288bdee..3c9fc25 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -86,7 +86,7 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000), m_robotDrive)); + .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); From 987a3a26472bec7bd0427c2525bfe85c1646042b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 18:36:04 -0700 Subject: [PATCH 41/49] Got DriveStraightAtVelocityPID Working --- src/main/java/frc4388/robot/Constants.java | 6 +- src/main/java/frc4388/robot/Robot.java | 2 - .../java/frc4388/robot/RobotContainer.java | 5 +- .../commands/DriveStraightAtVelocityPID.java | 51 +++++ ...D.java => DriveStraightToPositionPID.java} | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 201 +++++++++--------- 6 files changed, 155 insertions(+), 114 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java rename src/main/java/frc4388/robot/commands/{DriveToDistancePID.java => DriveStraightToPositionPID.java} (94%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 08a41df..27d5359 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,8 +29,8 @@ 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.0, 0.0, 0.0, 0.0448767878, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.0); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.1, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.3); 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 = 2000; public static final int DRIVE_ACCELERATION = 1000; @@ -50,7 +50,7 @@ public final class Constants { public final static int SLOT_MOTION_MAGIC = 3; /* Drive Train Characteristics */ - public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double TICKS_PER_MOTOR_REV = 2048*2; public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5; public static final double WHEEL_DIAMETER_INCHES = 6; public static final double TICKS_PER_GYRO_REV = 8192; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 41328b4..86b1d84 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -98,7 +98,6 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); - // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -113,7 +112,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3c9fc25..9b77168 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -17,7 +17,8 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; -import frc4388.robot.commands.DriveToDistancePID; +import frc4388.robot.commands.DriveStraightToPositionPID; +import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; @@ -79,7 +80,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveAtVelocityPID(m_robotDrive, 150)) + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java new file mode 100644 index 0000000..8179bed --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.subsystems.Drive; + +public class DriveStraightAtVelocityPID extends CommandBase { + Drive m_drive; + double m_targetVel; + double m_targetGyro; + /** + * Creates a new DriveVelocityControlPID. + */ + public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_targetVel = targetVel; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + m_drive.runVelocityPID(m_targetVel, m_targetGyro); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java similarity index 94% rename from src/main/java/frc4388/robot/commands/DriveToDistancePID.java rename to src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 22f8fa8..03e27d4 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; -public class DriveToDistancePID extends CommandBase { +public class DriveStraightToPositionPID extends CommandBase { Drive m_drive; double m_distance; double m_leftTarget; @@ -23,7 +23,7 @@ public class DriveToDistancePID extends CommandBase { * @param subsystem drive subsystem * @param distance distance to travel in inches */ - public DriveToDistancePID(Drive subsystem, double distance) { + public DriveStraightToPositionPID(Drive subsystem, double distance) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; m_distance = distance * DriveConstants.TICKS_PER_INCH; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 0258286..70f425d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -88,109 +88,117 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - //Beginning of nada + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + /* Setup Sensors for WPI_TalonFXs */ - //m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); - + m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + /* Configure the left Talon's selected sensor as local QuadEncoder */ - //m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source - // DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - // DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout /*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ - //m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source - // RemoteSensorSource.TalonSRX_SelectedSensor, - // DriveConstants.REMOTE_0, // Source number [0, 1] - // DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source + RemoteSensorSource.TalonSRX_SelectedSensor, + 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); + 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); + m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - //m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, - // DriveConstants.PID_PRIMARY, - // DriveConstants.DRIVE_TIMEOUT_MS); - - /* Scale Feedback by 0.5 to half the sum of Distance */ - /*m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient - DriveConstants.PID_PRIMARY, // PID Slot of Source - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - + /* Diff Signal */ m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); + /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, + DriveConstants.PID_PRIMARY, + DriveConstants.DRIVE_TIMEOUT_MS); + + /* Scale Feedback by 0.5 to half the sum of Distance */ + m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient + DriveConstants.PID_PRIMARY, // PID Slot of Source + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ - /*m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, + m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - /* Scale the Feedback Sensor using a coefficient */ - /*m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, + /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ + m_leftFrontMotor.configSelectedFeedbackCoefficient( 1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); - */ + /* 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_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); - + 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_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); + /* Smart Dashboard Initial Values */ /* Set up Chooser */ - //m_chooser.setDefaultOption("Distance PID", m_gainsDistance); + m_chooser.setDefaultOption("Distance PID", m_gainsDistance); //setDriveTrainGains("Distance PID", m_gainsDistance); - //m_chooser.addOption("Velocity PID", m_gainsVelocity); + m_chooser.addOption("Velocity PID", m_gainsVelocity); //setDriveTrainGains("Velocity PID", m_gainsVelocity); - //m_chooser.addOption("Turning PID", m_gainsTurning); + m_chooser.addOption("Turning PID", m_gainsTurning); //setDriveTrainGains("Turning PID", m_gainsTurning); - //m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic); + m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic); //setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic); - //Shuffleboard.getTab("PID").add(m_chooser); + Shuffleboard.getTab("PID").add(m_chooser); /* Gyro */ - //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); /* Sensor Values */ - //SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); - //SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); - //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); /* PID */ - //Gains gains = m_chooser.getSelected(); - //Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); - //Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); - //Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); - //Shuffleboard.getTab("PID").add("F Value Drive", gains.kF); + Gains gains = m_chooser.getSelected(); + Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); + Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); + Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); + Shuffleboard.getTab("PID").add("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(+1, DriveConstants.DRIVE_TIMEOUT_MS); - //m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); /** * 1ms per loop. PID loop can be slowed down if need be. @@ -199,56 +207,42 @@ public class Drive extends SubsystemBase { * - sensor deltas are very small per update, so derivative error never gets large enough to be useful. * - sensor movement is very slow causing the derivative error to be near zero. */ - //int closedLoopTimeMs = 1; - //m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - + int closedLoopTimeMs = 1; + 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(false, DriveConstants.DRIVE_TIMEOUT_MS); } - Gains gainsOld = m_chooser.getSelected(); @Override public void periodic() { try { - //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - //SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); - //SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - //SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); - //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - - /*Gains gains = m_chooser.getSelected(); - if (gains.equals(gainsOld)) { - SmartDashboard.putNumber("P Value Drive", gains.kP); - SmartDashboard.putNumber("I Value Drive", gains.kI); - SmartDashboard.putNumber("D Value Drive", gains.kD); - SmartDashboard.putNumber("F Value Drive", gains.kF); - } - gains.kP = SmartDashboard.getNumber("P Value Drive", gains.kP); - gains.kI = SmartDashboard.getNumber("I Value Drive", gains.kI); - gains.kD = SmartDashboard.getNumber("D Value Drive", gains.kD); - gains.kF = SmartDashboard.getNumber("F Value Drive", gains.kF); - setDriveTrainGains(m_chooser.getName(), gains); - gainsOld = gains;*/ + SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); + SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); + SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); } catch (Exception e) { System.err.println("Error in the Drive Subsystem"); @@ -328,15 +322,12 @@ public class Drive extends SubsystemBase { long last = 0; public void runVelocityPID(double targetVel, double targetGyro) { - //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightBackMotor.follow(m_rightFrontMotor); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, 1000);//, DemandType.AuxPID, targetGyro); - long now = System.nanoTime(); - SmartDashboard.putNumber("Loop Time", (now-last)/1000000); - last = now; - m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); - //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + targetVel *= 2; + m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); + //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); m_driveTrain.feedWatchdog(); } From 92c4a18c6e84c3f775960b00ab55965d2671d876 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 19:43:37 -0700 Subject: [PATCH 42/49] Finished Drive Straight Velocity PID and started Position PID --- .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/commands/DriveAtVelocityPID.java | 72 ------------------- .../commands/DriveStraightAtVelocityPID.java | 2 +- .../commands/DriveStraightToPositionPID.java | 25 +++---- .../java/frc4388/robot/subsystems/Drive.java | 27 ++++--- 5 files changed, 32 insertions(+), 100 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b77168..5981c2f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveStraightAtVelocityPID; @@ -68,9 +67,8 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive); + .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); /* Operator Buttons */ // activates "Lit Mode" @@ -80,7 +78,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000)) + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java deleted file mode 100644 index 7d241b6..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,72 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import com.ctre.phoenix.motorcontrol.FollowerType; -import com.ctre.phoenix.motorcontrol.NeutralMode; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; - -public class DriveAtVelocityPID extends CommandBase { - Drive m_drive; - double m_targetGyro; - double m_targetVel; - double m_leftTarget; - double m_rightTarget; - double m_copiedTargetVel; - /** - * Creates a new DriveAtVelocityPID. - * @param subsystem drive subsystem - * @param distance target velocity in inches/second - */ - 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_copiedTargetVel = targetVel; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_leftTarget = m_targetVel; - m_rightTarget = m_targetVel; - m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runVelocityPID(-20000, m_targetGyro); - - SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); - SmartDashboard.putNumber("Output Target Velocity", m_targetVel); - - SmartDashboard.putNumber("Target Angle", m_targetGyro); - //m_drive.runVelocityPID(m_leftTarget); - //m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_drive.setDriveTrainNeutralMode(NeutralMode.Coast); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java index 8179bed..e965036 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java @@ -35,7 +35,7 @@ public class DriveStraightAtVelocityPID extends CommandBase { @Override public void execute() { System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - m_drive.runVelocityPID(m_targetVel, m_targetGyro); + m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 03e27d4..a07846f 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -14,38 +14,33 @@ import frc4388.robot.subsystems.Drive; public class DriveStraightToPositionPID extends CommandBase { Drive m_drive; - double m_distance; - double m_leftTarget; - double m_rightTarget; + double m_targetPos; + double m_targetGyro; /** * Creates a new DriveToDistancePID. * @param subsystem drive subsystem - * @param distance distance to travel in inches + * @param targetPos distance to travel in inches */ - public DriveStraightToPositionPID(Drive subsystem, double distance) { + public DriveStraightToPositionPID(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_distance = distance * DriveConstants.TICKS_PER_INCH; + m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); - SmartDashboard.putNumber("Distance Target Inches", distance); + SmartDashboard.putNumber("Distance Target Inches", targetPos); } // Called when the command is initially scheduled. @Override public void initialize() { - m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - SmartDashboard.putNumber("Distance Target Ticks", m_distance); - SmartDashboard.putNumber("Left Target Ticks", m_leftTarget); - SmartDashboard.putNumber("Right Target Ticks", m_rightTarget); + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - //m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget); + System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro); } // Called once the command ends or is interrupted. @@ -56,7 +51,7 @@ public class DriveStraightToPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ + if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){ return true; } else { return false; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 70f425d..6a79518 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -93,7 +93,14 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); /* Setup Sensors for WPI_TalonFXs */ m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -313,21 +320,25 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void runPositionPID(WPI_TalonFX talon, double targetPos) { - talon.selectProfileSlot(DriveConstants.SLOT_DISTANCE , DriveConstants.PID_PRIMARY); - talon.set(TalonFXControlMode.Position, targetPos); + public void runDriveStraightPositionPID(double targetPos, double targetGyro) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + + targetPos *= 2; + m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); m_driveTrain.feedWatchdog(); } - long last = 0; - public void runVelocityPID(double targetVel, double targetGyro) { + public void runDriveStraightVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + targetVel *= 2; m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); - //m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_driveTrain.feedWatchdog(); } @@ -345,7 +356,7 @@ public class Drive extends SubsystemBase { */ public void runTurningPID(double targetAngle){ double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV; - runVelocityPID(0, targetGyro); + runDriveStraightVelocityPID(0, targetGyro); } public double getGyroYaw() { From f5106f42329bbed41006eaee2cf3c78f6b545485 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 19:57:25 -0700 Subject: [PATCH 43/49] Added command for storage to run --- .../java/frc4388/robot/RobotContainer.java | 5 +++++ .../frc4388/robot/subsystems/Storage.java | 20 +++++++++++++------ 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d52b23e..d54a017 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Storage; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -36,6 +37,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Storage m_robotStorage = new Storage(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -54,6 +56,9 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // runs storage motor at 50 percent + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.5), m_robotStorage)); + } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7b85a0d..31634ca 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -8,6 +8,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -18,18 +19,18 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { - private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_TALON_ID, MotorType.kBrushless); + private WPI_TalonSRX m_storageMotor = new WPI_TalonSRX(StorageConstants.STORAGE_TALON_ID); private DigitalInput[] m_beamSensors = new DigitalInput[6]; /** * Creates a new Storage. */ public Storage() { m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); + m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); + m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); + m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); + m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); } @Override @@ -44,5 +45,12 @@ public class Storage extends SubsystemBase { public void runStorage(double input) { m_storageMotor.set(input); boolean beam_on = m_beamSensors[0].get(); + + if (beam_on) { + System.err.println("Beam on"); + } else { + System.err.println("Beam off"); + } + } } From b7dfaf02481c89621820c063d21e8cfe67ae8953 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 19:59:41 -0700 Subject: [PATCH 44/49] Disables Not Working Buttons --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5981c2f..1fecc37 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,8 +67,8 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); + //new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + // .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); /* Operator Buttons */ // activates "Lit Mode" @@ -84,8 +84,8 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); + //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); From 315ed227036526f0d831e02942c9ecc3076a9c43 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 7 Feb 2020 17:02:43 -0700 Subject: [PATCH 45/49] Wrote Java Docs for PID Methods --- .../java/frc4388/robot/subsystems/Drive.java | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 6a79518..2a96a7c 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -269,7 +269,9 @@ public class Drive extends SubsystemBase { } /** - * Add your docs here. + * Initializes the drive train gains kP, kI, kD, and kF + * @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or "Turning PID" + * @param gains A gains object which is the gains that are set for the slot */ public void setDriveTrainGains(String slot, Gains gains){ /* Distance */ @@ -319,7 +321,11 @@ public class Drive extends SubsystemBase { public void driveWithInput(double move, double steer){ m_driveTrain.arcadeDrive(move, steer); } - + /** + * Runs a position PID while driving straight (has not been tested) + * @param targetPos The position to drive to in units + * @param targetGyro The angle to drive at in units + */ public void runDriveStraightPositionPID(double targetPos, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); @@ -330,7 +336,11 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - + /** + * Runs velocity PID while driving straight + * @param targetVel The velocity to drive at in units + * @param targetGyro The angle to drive at in units + */ public void runDriveStraightVelocityPID(double targetVel, double targetGyro) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); @@ -341,7 +351,11 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - + /** + * Runs motion magic PID while driving straight (has not been tested) + * @param targetPos The position to drive to in units + * @param targetGyro The angle to drive at in units + */ public void runMotionMagicPID(double targetPos, double targetGyro){ m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); From 479a7b93591503c3326057758ffba6a923f32f46 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 7 Feb 2020 17:50:23 -0700 Subject: [PATCH 46/49] FIxed code from Code Reviews --- src/main/java/frc4388/robot/Gains.java | 35 +++--- .../java/frc4388/robot/RobotContainer.java | 5 +- .../commands/DriveStraightAtVelocityPID.java | 6 +- .../commands/DriveStraightToPositionPID.java | 2 +- .../java/frc4388/robot/subsystems/Drive.java | 101 +++++++++++------- 5 files changed, 91 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/robot/Gains.java index 41c6422..b2f1de2 100644 --- a/src/main/java/frc4388/robot/Gains.java +++ b/src/main/java/frc4388/robot/Gains.java @@ -11,20 +11,29 @@ package frc4388.robot; * Add your docs here. */ public class Gains { - public double kP; - public double kI; - public double kD; - public double kF; - public int kIzone; - public double kPeakOutput; + public double m_kP; + public double m_kI; + public double m_kD; + public double m_kF; + public int m_kIzone; + public double m_kPeakOutput; - public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput) + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIzone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at. + */ + public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput) { - kP = _kP; - kI = _kI; - kD = _kD; - kF = _kF; - kIzone = _kIzone; - kPeakOutput = _kPeakOutput; + m_kP = kP; + m_kI = kI; + m_kD = kD; + m_kF = kF; + m_kIzone = kIzone; + m_kPeakOutput = kPeakOutput; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f2c8b5..2c196d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -77,16 +77,17 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); /* PID Test Command */ + // runs velocity PID while driving straight new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); - + // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); - + // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java index e965036..7b82ff8 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java @@ -16,7 +16,9 @@ public class DriveStraightAtVelocityPID extends CommandBase { double m_targetVel; double m_targetGyro; /** - * Creates a new DriveVelocityControlPID. + * Creates a new DriveStraightAtVelocityPID. + * @param subsystem The drive subsystem + * @param targetVel The target velocity for the motors in units */ public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) { // Use addRequirements() here to declare subsystem dependencies. @@ -34,7 +36,7 @@ public class DriveStraightAtVelocityPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index a07846f..67d78b3 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -39,7 +39,7 @@ public class DriveStraightToPositionPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2a96a7c..e038b29 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -60,8 +60,8 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configFactoryDefault(); m_leftBackMotor.configFactoryDefault(); m_rightBackMotor.configFactoryDefault(); - //m_pigeon.configFactoryDefault(); - //resetGyroYaw(); + m_pigeon.configFactoryDefault(); + resetGyroYaw(); /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); @@ -82,25 +82,25 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + 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.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + 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); /* Setup Sensors for WPI_TalonFXs */ m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); @@ -140,7 +140,7 @@ public class Drive extends SubsystemBase { DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); - /* Scale Feedback by 0.5 to half the sum of Distance */ + /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient DriveConstants.PID_PRIMARY, // PID Slot of Source DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout @@ -192,10 +192,10 @@ public class Drive extends SubsystemBase { /* PID */ Gains gains = m_chooser.getSelected(); - Shuffleboard.getTab("PID").add("P Value Drive", gains.kP); - Shuffleboard.getTab("PID").add("I Value Drive", gains.kI); - Shuffleboard.getTab("PID").add("D Value Drive", gains.kD); - Shuffleboard.getTab("PID").add("F Value Drive", gains.kF); + Shuffleboard.getTab("PID").add("P Value Drive", gains.m_kP); + Shuffleboard.getTab("PID").add("I Value Drive", gains.m_kI); + Shuffleboard.getTab("PID").add("D Value Drive", gains.m_kD); + Shuffleboard.getTab("PID").add("F Value Drive", gains.m_kF); /** @@ -277,38 +277,38 @@ public class Drive extends SubsystemBase { /* Distance */ if (slot.equals("Distance PID")) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); } /* Velocity */ if (slot.equals("Velocity PID")) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); } /* Turning */ if (slot.equals("Turning PID")) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); } /* Motion Magic */ if (slot.equals("Motion Magic PID")) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kD, 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); @@ -321,6 +321,7 @@ public class Drive extends SubsystemBase { public void driveWithInput(double move, double steer){ m_driveTrain.arcadeDrive(move, steer); } + /** * Runs a position PID while driving straight (has not been tested) * @param targetPos The position to drive to in units @@ -336,6 +337,7 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } + /** * Runs velocity PID while driving straight * @param targetVel The velocity to drive at in units @@ -351,6 +353,7 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } + /** * Runs motion magic PID while driving straight (has not been tested) * @param targetPos The position to drive to in units @@ -359,8 +362,10 @@ public class Drive extends SubsystemBase { public void runMotionMagicPID(double targetPos, double targetGyro){ m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_driveTrain.feedWatchdog(); } @@ -370,27 +375,43 @@ public class Drive extends SubsystemBase { */ public void runTurningPID(double targetAngle){ double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV; + runDriveStraightVelocityPID(0, targetGyro); } + /** + * Returns the current yaw of the pigeon + */ public double getGyroYaw() { double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); return ypr[0]; } + /** + * Returns the current pitch of the pigeon + */ public double getGyroPitch() { double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); return ypr[1]; } + /** + * Returns the current roll of the pigeon + */ public double getGyroRoll() { double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); return ypr[2]; } + /** + * Resets the yaw of the pigeon + */ public void resetGyroYaw() { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); From a3b0ac997665e9d78dea19ba3d9ba347136617ef Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 7 Feb 2020 17:52:52 -0700 Subject: [PATCH 47/49] Removed unused imports In Drive and Robot Container --- src/main/java/frc4388/robot/RobotContainer.java | 2 -- src/main/java/frc4388/robot/subsystems/Drive.java | 2 -- 2 files changed, 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2c196d9..d9c82a3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,8 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.DriveToDistanceMM; -import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index e038b29..ee725ad 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,7 +7,6 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.FollowerType; @@ -16,7 +15,6 @@ 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; From 1f37fe9f4a8d67120e90ec106d81c6593b878cb2 Mon Sep 17 00:00:00 2001 From: RishabhCowlagi <59751356+RishabhCowlagi@users.noreply.github.com> Date: Sat, 8 Feb 2020 12:02:07 -0800 Subject: [PATCH 48/49] Update Motor Type --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/subsystems/Storage.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index eb408fa..25912fc 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,7 +30,7 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } public static final class StorageConstants { - public static final int STORAGE_TALON_ID = 9; + public static final int STORAGE_CAN_ID = 10; public static final int BEAM_SENSOR_DIO_0 = 0; public static final int BEAM_SENSOR_DIO_1 = 1; public static final int BEAM_SENSOR_DIO_2 = 2; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 31634ca..a7d1bfb 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { - private WPI_TalonSRX m_storageMotor = new WPI_TalonSRX(StorageConstants.STORAGE_TALON_ID); + private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); private DigitalInput[] m_beamSensors = new DigitalInput[6]; /** * Creates a new Storage. From 33f974d6d90f9103365f7362fc6d6fb9b4f0dba3 Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Sat, 8 Feb 2020 14:05:12 -0700 Subject: [PATCH 49/49] Changed IDs to make storage subsystem work --- src/main/java/frc4388/robot/Constants.java | 6 +++--- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 25912fc..db3c0f2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,13 +30,13 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } public static final class StorageConstants { - public static final int STORAGE_CAN_ID = 10; + public static final int STORAGE_CAN_ID = 9; public static final int BEAM_SENSOR_DIO_0 = 0; public static final int BEAM_SENSOR_DIO_1 = 1; public static final int BEAM_SENSOR_DIO_2 = 2; public static final int BEAM_SENSOR_DIO_3 = 3; - public static final int BEAM_SENSOR_DIO_4 = 0; - public static final int BEAM_SENSOR_DIO_5 = 0; + public static final int BEAM_SENSOR_DIO_4 = 4; + public static final int BEAM_SENSOR_DIO_5 = 5; } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d54a017..c75e34c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -57,7 +57,7 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs storage motor at 50 percent - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.5), m_robotStorage)); + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); }