From 18c21fdb7e84dc713e2e4250852d77c1be472821 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 13 Jan 2020 17:53:42 -0700 Subject: [PATCH 01/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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/96] 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 2eb0bf806a76635812cfdac0926a603e7336cfda Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 08:29:34 -0800 Subject: [PATCH 14/96] Created Elevator Subsystem Initialized motors --- .../frc4388/robot/subsystems/Elevator.java | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Elevator.java diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java new file mode 100644 index 0000000..19712e8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* 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.WPI_TalonSRX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Elevator extends SubsystemBase { + + public WPI_TalonSRX m_talon1; + public WPI_TalonSRX m_talon2; + + /** + * Creates a new Elevator. + */ + public Elevator() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} 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 15/96] 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 ca31832c6197f9e659915b74144bda56837ab2f6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 10:30:22 -0800 Subject: [PATCH 16/96] Added Elevator Subystem Added method to make it move --- .../java/frc4388/robot/RobotContainer.java | 6 +++++ .../frc4388/robot/subsystems/Elevator.java | 24 ++++++++++++++++--- 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 202f647..eb93394 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.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; @@ -38,6 +39,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 Elevator m_robotElevator = new Elevator(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -56,6 +58,10 @@ public class RobotContainer { getDriverController().getRightXAxis()), m_robotDrive)); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // moves elevator with one-axis input from the driver controller + m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( + getOperatorController().getLeftYAxis()), m_robotElevator + )); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 19712e8..4af5ef1 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -7,24 +7,42 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ElevatorConstants; public class Elevator extends SubsystemBase { - - public WPI_TalonSRX m_talon1; - public WPI_TalonSRX m_talon2; + + public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); + public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); /** * Creates a new Elevator. */ public Elevator() { + m_talon1.configFactoryDefault(); + m_talon2.configFactoryDefault(); + m_talon2.follow(m_talon1); + + m_talon1.setNeutralMode(NeutralMode.Brake); + m_talon2.setNeutralMode(NeutralMode.Brake); + + m_talon1.setInverted(false); + m_talon2.setInverted(false); + m_talon1.setInverted(InvertType.FollowMaster); + m_talon2.setInverted(InvertType.FollowMaster); } @Override public void periodic() { // This method will be called once per scheduler run } + public void moveElevator(double speed) { + m_talon1.set(speed); + m_talon2.set(speed); + } } From d4be141676055a545e1e6209804be0b4326e6d48 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 10:30:57 -0800 Subject: [PATCH 17/96] Changed Constants --- src/main/java/frc4388/robot/Constants.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 466ff74..b3409f0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -38,6 +38,11 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } + public static final class ElevatorConstants { + public static final int TALON_1 = 7; + public static final int TALON_2 = 8; + } + public static final class LEDConstants { public static final int LED_SPARK_ID = 0; From 162bbac7bcd317e34ede70bf08cac8cea94a23da Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 12:47:49 -0700 Subject: [PATCH 18/96] 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 19/96] 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 20/96] 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 21/96] 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 101cfb5d10f496315ce7bfaff4ba003cfed626dc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Jan 2020 14:29:30 -0800 Subject: [PATCH 22/96] Finished Elevator Robot Challenge --- src/main/java/frc4388/robot/Constants.java | 8 +++ .../java/frc4388/robot/RobotContainer.java | 4 ++ .../robot/commands/DistanceElevatorPID.java | 56 +++++++++++++++++++ .../robot/commands/DriveAtVelocityPID.java | 4 +- .../robot/commands/DriveToDistanceMM.java | 4 +- .../robot/commands/DriveToDistancePID.java | 4 +- .../java/frc4388/robot/subsystems/Drive.java | 6 +- .../frc4388/robot/subsystems/Elevator.java | 28 ++++++++++ 8 files changed, 105 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DistanceElevatorPID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b3409f0..2714b3b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -41,6 +41,14 @@ public final class Constants { public static final class ElevatorConstants { public static final int TALON_1 = 7; public static final int TALON_2 = 8; + + /* PID Constants Elevator */ + public static final int ELEVATOR_SLOT_IDX = 0; + public static final int ELEVATOR_PID_LOOP_IDX = 1; + public static final int ELEVATOR_TIMEOUT_MS = 30; + public static final Gains ELEVATOR_GAINS = 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index eb93394..25f9528 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.DistanceElevatorPID; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; @@ -94,6 +95,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); + new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive)); } diff --git a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java b/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java new file mode 100644 index 0000000..10644ca --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Elevator; + +public class DistanceElevatorPID extends CommandBase { + Elevator m_elevator; + double m_distance; + double m_target1; + double m_target2; + /** + * Creates a new DistanceElevatorPID. + */ + public DistanceElevatorPID(Elevator subsystem, double distance) { + // Use addRequirements() here to declare subsystem dependencies. + m_elevator = subsystem; + m_distance = distance; + addRequirements(m_elevator); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_target1 = m_elevator.m_talon1.getActiveTrajectoryPosition() + m_distance; + m_target2 = m_elevator.m_talon2.getActiveTrajectoryPosition() + m_distance; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_elevator.runElevatorPositionPID(m_elevator.m_talon1, m_target1); + m_elevator.runElevatorPositionPID(m_elevator.m_talon2, m_target2); + } + + // 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_elevator.m_talon1.getActiveTrajectoryPosition() - m_target1) < 100) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index 70e54dd..3d3e96c 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -35,8 +35,8 @@ 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_leftFrontMotor, m_leftTarget); - m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); + m_drive.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDriveVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java index 0faf431..df7ff44 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java @@ -36,8 +36,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.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDriveMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java index 3fe0ea0..79b27e9 100644 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -39,8 +39,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.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDrivePositionPID(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 2ad83d9..833f132 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -170,15 +170,15 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void runPositionPID(WPI_TalonFX talon, double targetPos) { + public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) { talon.set(TalonFXControlMode.Position, targetPos); } - public void runVelocityPID(WPI_TalonFX talon, double targetVel) { + public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) { talon.set(TalonFXControlMode.Velocity, targetVel); } - public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){ + public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){ talon.set(TalonFXControlMode.MotionMagic, targetPos); } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4af5ef1..4423466 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -7,11 +7,13 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; import frc4388.robot.Constants.ElevatorConstants; public class Elevator extends SubsystemBase { @@ -19,13 +21,16 @@ public class Elevator extends SubsystemBase { public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); + public static Gains m_gains = ElevatorConstants.ELEVATOR_GAINS; /** * Creates a new Elevator. */ public Elevator() { + //resets motors m_talon1.configFactoryDefault(); m_talon2.configFactoryDefault(); + //config following settings m_talon2.follow(m_talon1); m_talon1.setNeutralMode(NeutralMode.Brake); @@ -35,6 +40,13 @@ public class Elevator extends SubsystemBase { m_talon2.setInverted(false); m_talon1.setInverted(InvertType.FollowMaster); m_talon2.setInverted(InvertType.FollowMaster); + + m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + + int closedLoopTimeMs = 1; + m_talon1.configClosedLoopPeriod(0, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.configClosedLoopPeriod(1, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); } @Override @@ -45,4 +57,20 @@ public class Elevator extends SubsystemBase { m_talon1.set(speed); m_talon2.set(speed); } + public void setElevatorGains(){ + m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); + m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + + m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); + m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + } + public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { + talon.set(ControlMode.Position, targetPos); + } } From 41ab970692564f13f18231fa62c2a1c02276baf5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 18 Jan 2020 16:12:56 -0700 Subject: [PATCH 23/96] 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 d9bd213ff29377c5b2dc21a2b4190e0812758e21 Mon Sep 17 00:00:00 2001 From: Kyra <101209@psdschools.org> Date: Sat, 18 Jan 2020 15:28:48 -0800 Subject: [PATCH 24/96] Create Shooter Subsystem and PID stuff Created shooter subsystem, set up command for shooter to run, set up PID constants, set up gains method in shooter. --- src/main/java/frc4388/robot/Constants.java | 12 ++++ .../java/frc4388/robot/RobotContainer.java | 11 +++- .../java/frc4388/robot/subsystems/Drive.java | 26 ++++----- .../frc4388/robot/subsystems/Elevator.java | 20 ++++--- .../frc4388/robot/subsystems/Shooter.java | 57 +++++++++++++++++++ 5 files changed, 102 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Shooter.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2714b3b..9dbda6a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,6 +51,18 @@ public final class Constants { public static final double ENCODER_TICKS_PER_REV = 2048; } + public static final class ShooterConstants { + public static final int SHOOTER_FALCON_ID = 8; + + /* PID Constants Shooter */ + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final int SHOOTER_TIMEOUT_MS = 30; + public static final Gains SHOOTER_GAINS = 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 { 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 25f9528..abdfb35 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Shooter; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -41,6 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); private final Elevator m_robotElevator = new Elevator(); + private final Shooter m_robotShooter = new Shooter(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -57,12 +59,15 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.driveWithInput( getDriverController().getLeftYAxis(), getDriverController().getRightXAxis()), m_robotDrive)); - // drives motor with input from triggers on the opperator controller + + // drives motor with input from triggers on the operator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // moves elevator with one-axis input from the driver controller m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( getOperatorController().getLeftYAxis()), m_robotElevator )); + // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } @@ -85,7 +90,6 @@ 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(new DriveToDistancePID(m_robotDrive, 5000)); @@ -100,6 +104,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive)); + + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 833f132..df272f9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,7 +34,7 @@ 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_driveGains = DriveConstants.DRIVE_GAINS; /** * Add your docs here. @@ -121,10 +121,10 @@ 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_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); setDriveTrainGains(); @@ -151,16 +151,16 @@ public class Drive extends SubsystemBase { */ 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_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.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); + m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4423466..eec7909 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -21,7 +21,7 @@ public class Elevator extends SubsystemBase { public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); - public static Gains m_gains = ElevatorConstants.ELEVATOR_GAINS; + public static Gains m_elevatorGains = ElevatorConstants.ELEVATOR_GAINS; /** * Creates a new Elevator. */ @@ -41,6 +41,8 @@ public class Elevator extends SubsystemBase { m_talon1.setInverted(InvertType.FollowMaster); m_talon2.setInverted(InvertType.FollowMaster); + setElevatorGains(); + m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); @@ -59,16 +61,16 @@ public class Elevator extends SubsystemBase { } public void setElevatorGains(){ m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); + m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); } public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { talon.set(ControlMode.Position, targetPos); diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java new file mode 100644 index 0000000..eb1b6d3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.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.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; +import frc4388.robot.Constants.ShooterConstants; + +public class Shooter extends SubsystemBase { + + WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + + /** + * Creates a new Shooter. + */ + public Shooter() { + m_shooterFalcon.configFactoryDefault(); + + m_shooterFalcon.setNeutralMode(NeutralMode.Coast); + + m_shooterFalcon.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs drum shooter motor. + * @param speed + */ + public void runDrumShooter(double speed) { + m_shooterFalcon.set(speed); + } + + /** + * Configures gains for shooter PID. + */ + public void setShooterGains() { + m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } +} From de4265be232a8524c6902b8b2718856f4c4eecbd Mon Sep 17 00:00:00 2001 From: aarav18 <57469559+aarav18@users.noreply.github.com> Date: Sun, 19 Jan 2020 00:28:21 -0700 Subject: [PATCH 25/96] Added Shooter Subsystem PID Stuff Initialized gains and added velocity PID method. --- .../java/frc4388/robot/subsystems/Shooter.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index eb1b6d3..934cbae 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -29,6 +29,14 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(false); + + setShooterGains(); + + m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + int closedLoopTimeMs = 1; + m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); } @Override @@ -38,7 +46,7 @@ public class Shooter extends SubsystemBase { /** * Runs drum shooter motor. - * @param speed + * @param speed Speed to set the motor at */ public void runDrumShooter(double speed) { m_shooterFalcon.set(speed); @@ -54,4 +62,12 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } + /** + * Runs drum shooter velocity PID. + * @param falcon Motor to use + * @param targetVel Target velocity to run motor at + */ + public void runDrumShooterVelocityPID(WPI_TalonFX falcon, double targetVel) { + falcon.set(TalonFXControlMode.Velocity, targetVel); + } } From a96cd7a3e2189695446895be06591a63b9097ffb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 20 Jan 2020 08:59:20 -0800 Subject: [PATCH 26/96] Finished up Velocity Control PID Shooter PID --- .../java/frc4388/robot/RobotContainer.java | 6 ++- .../commands/ShooterVelocityControlPID.java | 47 +++++++++++++++++++ .../frc4388/robot/subsystems/Shooter.java | 3 +- 3 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index abdfb35..d4902b2 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.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; @@ -99,13 +100,16 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2000)); + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive)); - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java new file mode 100644 index 0000000..dcc0fb3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Shooter; + +public class ShooterVelocityControlPID extends CommandBase { + Shooter m_shooter; + double m_targetVel; + /** + * Creates a new ShooterVelocityControlPID. + */ + public ShooterVelocityControlPID(Shooter subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_shooter = subsystem; + m_targetVel = targetVel; + addRequirements(m_shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_shooter.runDrumShooterVelocityPID(m_shooter.m_shooterFalcon, m_targetVel); + } + + // 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/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 934cbae..8e7cd9c 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -8,6 +8,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -16,7 +17,7 @@ import frc4388.robot.Constants.ShooterConstants; public class Shooter extends SubsystemBase { - WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; From f5fa226d98591a74f45d40c462227adcf50a1eac Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 20 Jan 2020 10:40:12 -0700 Subject: [PATCH 27/96] 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 28/96] 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 29/96] 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 30/96] 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 db5c15a452143a55f64a23e284e69a2fed03018b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 20 Jan 2020 15:22:44 -0700 Subject: [PATCH 31/96] Tested Shooter, Fine tuned it --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++---- .../robot/commands/ShooterVelocityControlPID.java | 2 +- src/main/java/frc4388/robot/subsystems/Shooter.java | 13 ++++++++++--- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d4902b2..9c45f3e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,16 +101,16 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2000)); + .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(2300), m_robotShooter)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); - 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));*/ new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(() -> m_robotShooter.runDrumShooter(1), m_robotShooter); + .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); } /** diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index dcc0fb3..520e501 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,7 +31,7 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_shooter.m_shooterFalcon, m_targetVel); + m_shooter.runDrumShooterVelocityPID(m_targetVel); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8e7cd9c..24b7461 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -11,14 +11,18 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.ShooterConstants; public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + private double m_targetVel = 2300; + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; /** @@ -29,7 +33,7 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setNeutralMode(NeutralMode.Coast); - m_shooterFalcon.setInverted(false); + m_shooterFalcon.setInverted(true); setShooterGains(); @@ -38,11 +42,14 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + SmartDashboard.putNumber("Shooter Velocity", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run + m_targetVel = SmartDashboard.getNumber("Shooter Velocity", m_targetVel); } /** @@ -68,7 +75,7 @@ public class Shooter extends SubsystemBase { * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ - public void runDrumShooterVelocityPID(WPI_TalonFX falcon, double targetVel) { - falcon.set(TalonFXControlMode.Velocity, targetVel); + public void runDrumShooterVelocityPID(double targetVel) { + m_shooterFalcon.set(TalonFXControlMode.Velocity, m_targetVel*ShooterConstants.ENCODER_TICKS_PER_REV/600); } } From 7f96cecc4d95c2dbae2873f4b6b034c37fffcdb9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 21 Jan 2020 16:45:29 -0700 Subject: [PATCH 32/96] Spin up before engaging shooter pid --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/ShooterVelocityControlPID.java | 6 +++++- src/main/java/frc4388/robot/subsystems/Shooter.java | 5 +++-- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9dbda6a..835dff1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,7 +46,7 @@ public final class Constants { public static final int ELEVATOR_SLOT_IDX = 0; public static final int ELEVATOR_PID_LOOP_IDX = 1; public static final int ELEVATOR_TIMEOUT_MS = 30; - public static final Gains ELEVATOR_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } @@ -58,7 +58,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 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 9c45f3e..99b7d52 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,7 +101,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(2300), m_robotShooter)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2300)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 520e501..4c7b058 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,7 +31,11 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_targetVel); + if (m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000){ + m_shooter.runDrumShooter(0.5); + } else { + m_shooter.runDrumShooterVelocityPID(m_targetVel); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 24b7461..9452170 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -43,13 +43,14 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - SmartDashboard.putNumber("Shooter Velocity", m_targetVel); + SmartDashboard.putNumber("Shooter Velocity Target", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run - m_targetVel = SmartDashboard.getNumber("Shooter Velocity", m_targetVel); + m_targetVel = SmartDashboard.getNumber("Shooter Velocity Target", m_targetVel); + SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } /** From 7008ceb4539bfda74b1e5323331386a098c71f9a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 23 Jan 2020 16:42:20 -0700 Subject: [PATCH 33/96] 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 34/96] 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 35/96] 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 36/96] 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 66893b5890d928a4aeee831aa46c2a27402a5fd1 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 28 Jan 2020 18:56:09 -0800 Subject: [PATCH 37/96] Added climber subsystem --- src/main/java/frc4388/robot/Constants.java | 4 ++ .../java/frc4388/robot/RobotContainer.java | 5 ++ .../commands/RunClimberWithTriggers.java | 63 +++++++++++++++++ .../frc4388/robot/subsystems/Climber.java | 46 ++++++++++++ vendordeps/Phoenix.json | 30 ++++---- vendordeps/REVRobotics.json | 70 +++++++++++++++++++ 6 files changed, 203 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.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..da1a079 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,6 +29,10 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 1; } + + public static final class ClimberConstants { + public static final int CLIMBER_SPARK_ID = 10; + } 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 d52b23e..82188ef 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,7 +16,9 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -36,6 +38,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 Climber m_robotClimber = new Climber(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -52,6 +55,8 @@ public class RobotContainer { 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())); + // drives motor with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } diff --git a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java new file mode 100644 index 0000000..9ab4285 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Climber; +import frc4388.utility.controller.IHandController; + +public class RunClimberWithTriggers extends CommandBase { + private Climber m_climber; + private IHandController m_controller; + + /** + * Uses input from opperator triggers to control climber motor + * @param subsystem the climber subsystem + * @param controller the driver controller + */ + public RunClimberWithTriggers(Climber subsystem, IHandController controller) { + m_climber = subsystem; + m_controller = controller; + addRequirements(m_climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double rightTrigger = m_controller.getRightTriggerAxis(); + double leftTrigger = m_controller.getLeftTriggerAxis(); + double output = 0; + if (rightTrigger < .5) { + if(rightTrigger > leftTrigger) { + output = rightTrigger; + } + if (leftTrigger > rightTrigger) { + output = -leftTrigger; + } + } else { + output = rightTrigger; + } + m_climber.runClimber(output); + } + + // 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/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..de0b618 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; + +public class Climber extends SubsystemBase { + CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_forwardLimit, m_reverseLimit; + /** + * Creates a new Climber. + */ + public Climber() { + m_climberMotor.restoreFactoryDefaults(); + + m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + + m_forwardLimit.enableLimitSwitch(true); + m_reverseLimit.enableLimitSwitch(true); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs climber motor + * @param input the voltage to run motor at + */ + public void runClimber(double input) { + m_climberMotor.set(input); + } +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index f8d42a4..a633555 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.3", + "version": "5.17.4", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.3" + "version": "5.17.4" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.3" + "version": "5.17.4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, 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 38/96] 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 39/96] 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 40/96] 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 7be98f8d3096112d9c09d8fe88516ea371897c56 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 21:59:12 -0700 Subject: [PATCH 41/96] Update Intake documentation --- src/main/java/frc4388/robot/subsystems/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index f718cb6..ea2ba9c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -28,7 +28,7 @@ public class Intake extends SubsystemBase { /** * Runs intake motor - * @param input the voltage to run motor at + * @param input the percent output to run motor at */ public void runIntake(double input) { m_intakeMotor.set(input); From 6f0a052b0ade51e94f76b8c706dd1178d91fa7d0 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:00:52 -0700 Subject: [PATCH 42/96] Update LED documentation --- src/main/java/frc4388/robot/subsystems/LED.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index a68c124..6ef70e5 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -14,17 +14,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; import frc4388.utility.LEDPatterns; -/** - * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED - * Driver - */ public class LED extends SubsystemBase { public static float currentLED; public static Spark LEDController; /** - * Add your docs here. + * Creates a new LED to run a 5v LED Strip using a Rev + * Robotics Blinkin LED Driver */ public LED(){ LEDController = new Spark(LEDConstants.LED_SPARK_ID); @@ -34,14 +31,17 @@ public class LED extends SubsystemBase { } /** - * Add your docs here. + * Sends an update to the LED Driver with the current LED value. + * This method should be run continously to keep the lights on. */ public void updateLED(){ LEDController.set(currentLED); } /** - * Add your docs here. + * Sets the current LED pattern. This method should only be run + * whenever you want to change the current LED. + * @param pattern LEDPattern to set the Blinkin to. */ public void setPattern(LEDPatterns pattern){ currentLED = pattern.getValue(); From aac97078424e2c9d0682b025dd6c79bb371b3571 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:11:48 -0700 Subject: [PATCH 43/96] Update DriveWithJoystick.java --- .../java/frc4388/robot/commands/DriveWithJoystick.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 2b31457..e663fdb 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -16,7 +16,12 @@ public class DriveWithJoystick extends CommandBase { private IHandController m_controller; /** - * Creates a new DriveWithJoystick. + * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller. + * Applies a curved ramp to the input from the controllers to make the robot less "touchy". + * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ public DriveWithJoystick(Drive subsystem, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. From a46ec9ff14ab58fe48b174e9bfd961fd5790c713 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:15:03 -0700 Subject: [PATCH 44/96] Update RunIntakeWithTriggers docs and fix potential bug --- .../frc4388/robot/commands/RunIntakeWithTriggers.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java index 359ea0e..bb7cef2 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java @@ -16,9 +16,12 @@ public class RunIntakeWithTriggers extends CommandBase { private IHandController m_controller; /** - * Uses input from opperator triggers to control intake motor - * @param subsystem the intake subsystem - * @param controller the operator controller + * Uses input from opperator triggers to control intake motor. + * The right trigger will run the intake in and the left trigger will run it out. + * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ public RunIntakeWithTriggers(Intake subsystem, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. @@ -43,7 +46,7 @@ public class RunIntakeWithTriggers extends CommandBase { output = rightTrigger; } if (leftTrigger > rightTrigger) { - output = leftTrigger; + output = -leftTrigger; } } else { output = rightTrigger; From f8fc2d3b069c44ef2734195ea7c6db877eba71fb Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:28:02 -0700 Subject: [PATCH 45/96] Update Controller docs in Robot Container --- src/main/java/frc4388/robot/RobotContainer.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d52b23e..4131d4b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -94,14 +94,16 @@ public class RobotContainer { } /** - * Add your docs here. + * Used for analog inputs like triggers and axises. + * @return IHandController interface for the Driver Controller. */ public IHandController getDriverController() { return m_driverXbox; } /** - * Add your docs here. + * Used for analog inputs like triggers and axises. + * @return The IHandController interface for the Operator Controller. */ public IHandController getOperatorController() { @@ -109,7 +111,9 @@ public class RobotContainer { } /** - * Add your docs here. + * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller. + * Generic HIDs/Joysticks can be used to set up JoystickButtons. + * @return The IHandController interface for the Operator Controller. */ public Joystick getOperatorJoystick() { @@ -117,7 +121,9 @@ public class RobotContainer { } /** - * Add your docs here. + * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Driver Xbox Controller. + * Generic HIDs/Joysticks can be used to set up JoystickButtons. + * @return The IHandController interface for the Driver Controller. */ public Joystick getDriverJoystick() { From 63b9831a9a8577d9c50daea16c6d925881542bcb Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:31:50 -0700 Subject: [PATCH 46/96] Update XboxController docs --- .../java/frc4388/utility/controller/XboxController.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 8b8f0f8..4353d1e 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -3,7 +3,7 @@ package frc4388.utility.controller; import edu.wpi.first.wpilibj.Joystick; /** - * This is a wrapper for the WPILib Joystick class that represents an XBox + * This is a wrapper for the WPILib Joystick class that represents an Xbox * controller. * @author frc1675 */ @@ -53,14 +53,15 @@ public class XboxController implements IHandController private Joystick m_stick; /** - * Add your docs here. - */ + * Creates a new Xbox Controller + * @param portNumber ID of Xbox Controller + */ public XboxController(int portNumber){ m_stick = new Joystick(portNumber); } /** - * Add your docs here. + * @return Joystick for Xbox Controller */ public Joystick getJoyStick() { return m_stick; From 08adb765f8682570ad79ee565a61add0c2213198 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:32:56 -0700 Subject: [PATCH 47/96] Remove uneeded inherit in XboxTriggerButton --- src/main/java/frc4388/utility/controller/XboxTriggerButton.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 26372c2..0339085 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -31,7 +31,6 @@ public class XboxTriggerButton extends Button { m_trigger = trigger; } - /** {@inheritDoc} */ @Override public boolean get() { if (m_trigger == RIGHT_TRIGGER) { From 3fa0bc7dc8c6fbe9d36ade628a048629fd9b7c50 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:36:12 -0700 Subject: [PATCH 48/96] Update IHandController.java --- src/main/java/frc4388/utility/controller/IHandController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java index 13aa763..b54333c 100644 --- a/src/main/java/frc4388/utility/controller/IHandController.java +++ b/src/main/java/frc4388/utility/controller/IHandController.java @@ -1,7 +1,7 @@ package frc4388.utility.controller; /** - * Add your docs here. + * Interface for the {@link XboxController}. */ public interface IHandController { From bbecb95ae094aa967f660ce397ff9485def9791d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:36:22 -0700 Subject: [PATCH 49/96] Update LEDPatterns.java --- src/main/java/frc4388/utility/LEDPatterns.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java index 6df032c..8c98acc 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/LEDPatterns.java @@ -1,7 +1,8 @@ package frc4388.utility; /** - * Add your docs here. + * Enum to hold all the different patterns for a Blinkin LED Driver. + * Use in place of a double when setting the Blinkin output. */ public enum LEDPatterns { /* PALLETTE PATTERNS */ From 0c6827e4ad70e0ebab328292fe06dd2e1d31a1d9 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 22:37:29 -0700 Subject: [PATCH 50/96] Update XboxTriggerButton.java --- src/main/java/frc4388/utility/controller/XboxTriggerButton.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 0339085..9c3cb2c 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -5,7 +5,7 @@ import frc4388.utility.controller.XboxController; /** * Mapping for the Xbox controller triggers to allow triggers to be defined as - * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger + * buttons in {@link frc4388.robot.RobotContainer RobotContainer}. Checks to see if the given trigger * exceeds a tolerance defined in {@link XboxController}. */ public class XboxTriggerButton extends Button { From 009443b0446a594674d7af2e39b5d1477c321979 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 30 Jan 2020 23:21:35 -0700 Subject: [PATCH 51/96] 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 52/96] 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 53/96] 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 42db77fe6d596ced6bc5e1303090f716a7a150ef Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 21:39:51 -0700 Subject: [PATCH 54/96] Fix Shooter PID --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 13 +--- .../robot/commands/DistanceElevatorPID.java | 56 ------------- .../commands/ShooterVelocityControlPID.java | 5 +- .../frc4388/robot/subsystems/Elevator.java | 78 ------------------- .../frc4388/robot/subsystems/Shooter.java | 13 +++- 6 files changed, 16 insertions(+), 151 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DistanceElevatorPID.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Elevator.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 835dff1..4ae4474 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.0542, 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 86be53e..e9527ae 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.DistanceElevatorPID; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; @@ -23,7 +22,6 @@ import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; @@ -43,7 +41,6 @@ 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 Elevator m_robotElevator = new Elevator(); private final Shooter m_robotShooter = new Shooter(); /* Controllers */ @@ -61,12 +58,9 @@ public class RobotContainer { 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())); - // moves elevator with one-axis input from the driver controller - m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( - getOperatorController().getLeftYAxis()), m_robotElevator - )); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); } /** @@ -97,10 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2300)); - - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 3200)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java b/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java deleted file mode 100644 index 10644ca..0000000 --- a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java +++ /dev/null @@ -1,56 +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 edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Elevator; - -public class DistanceElevatorPID extends CommandBase { - Elevator m_elevator; - double m_distance; - double m_target1; - double m_target2; - /** - * Creates a new DistanceElevatorPID. - */ - public DistanceElevatorPID(Elevator subsystem, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - m_elevator = subsystem; - m_distance = distance; - addRequirements(m_elevator); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_target1 = m_elevator.m_talon1.getActiveTrajectoryPosition() + m_distance; - m_target2 = m_elevator.m_talon2.getActiveTrajectoryPosition() + m_distance; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_elevator.runElevatorPositionPID(m_elevator.m_talon1, m_target1); - m_elevator.runElevatorPositionPID(m_elevator.m_talon2, m_target2); - } - - // 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_elevator.m_talon1.getActiveTrajectoryPosition() - m_target1) < 100) { - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 4c7b058..dbd6603 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,10 +31,11 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000){ + if (/*m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000*/false){ m_shooter.runDrumShooter(0.5); + System.err.println("Less than 1000: " + m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } else { - m_shooter.runDrumShooterVelocityPID(m_targetVel); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java deleted file mode 100644 index eec7909..0000000 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ /dev/null @@ -1,78 +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.subsystems; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; -import frc4388.robot.Constants.ElevatorConstants; - -public class Elevator extends SubsystemBase { - - public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); - public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); - - public static Gains m_elevatorGains = ElevatorConstants.ELEVATOR_GAINS; - /** - * Creates a new Elevator. - */ - public Elevator() { - //resets motors - m_talon1.configFactoryDefault(); - m_talon2.configFactoryDefault(); - - //config following settings - m_talon2.follow(m_talon1); - - m_talon1.setNeutralMode(NeutralMode.Brake); - m_talon2.setNeutralMode(NeutralMode.Brake); - - m_talon1.setInverted(false); - m_talon2.setInverted(false); - m_talon1.setInverted(InvertType.FollowMaster); - m_talon2.setInverted(InvertType.FollowMaster); - - setElevatorGains(); - - m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - - int closedLoopTimeMs = 1; - m_talon1.configClosedLoopPeriod(0, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.configClosedLoopPeriod(1, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - public void moveElevator(double speed) { - m_talon1.set(speed); - m_talon2.set(speed); - } - public void setElevatorGains(){ - m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - - m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - } - public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { - talon.set(ControlMode.Position, targetPos); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 9452170..5fe805a 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -25,6 +25,7 @@ public class Shooter extends SubsystemBase { public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + double velP; /** * Creates a new Shooter. */ @@ -76,7 +77,13 @@ public class Shooter extends SubsystemBase { * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ - public void runDrumShooterVelocityPID(double targetVel) { - m_shooterFalcon.set(TalonFXControlMode.Velocity, m_targetVel*ShooterConstants.ENCODER_TICKS_PER_REV/600); + public void runDrumShooterVelocityPID(double targetVel, double actualVel) { + velP = actualVel/targetVel; + if(velP < 0.1){ + velP = 0.1; + } + double runSpeed = velP*(1-velP); + System.err.println(runSpeed); + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); } -} +} \ No newline at end of file From 8e7e5218125370745c2b23bcd96419e7d169b613 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 21:43:46 -0700 Subject: [PATCH 55/96] Update build.gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 68eb59bdc687fc45d1932062f0fd90c7fca46844 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 31 Jan 2020 22:02:39 -0700 Subject: [PATCH 56/96] Fix the thing and that other thing --- .../frc4388/robot/commands/ShooterVelocityControlPID.java | 8 ++------ src/main/java/frc4388/robot/subsystems/Shooter.java | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index dbd6603..799163b 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,12 +31,8 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (/*m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000*/false){ - m_shooter.runDrumShooter(0.5); - System.err.println("Less than 1000: " + m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); - } else { - m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); - } + System.err.println(m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 5fe805a..6cfd3be 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -83,7 +83,7 @@ public class Shooter extends SubsystemBase { velP = 0.1; } double runSpeed = velP*(1-velP); - System.err.println(runSpeed); + //System.err.println(runSpeed); m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); } } \ No newline at end of file From 43a454f9ded01eb4ee37234e5a8cac205184ea5a Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 1 Feb 2020 12:40:39 -0700 Subject: [PATCH 57/96] Runup --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- .../robot/commands/ShooterVelocityControlPID.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Shooter.java | 11 +++++------ 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e9527ae..153c296 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,7 @@ 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)); - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); } /** @@ -91,7 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 3200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 799163b..67b5cb1 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,8 +31,8 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println(m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); - m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); + System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 6cfd3be..fdbc39e 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -79,11 +79,10 @@ public class Shooter extends SubsystemBase { */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { velP = actualVel/targetVel; - if(velP < 0.1){ - velP = 0.1; - } - double runSpeed = velP*(1-velP); - //System.err.println(runSpeed); - m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); + double runSpeed = actualVel + (20000*velP); + if (runSpeed > targetVel) {runSpeed = targetVel;} + System.err.println(velP + " " + runSpeed); + SmartDashboard.putNumber("shoot", actualVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); } } \ No newline at end of file From 4dd70bf1fe4fe9156c4883f7b248151f8fe50f42 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 1 Feb 2020 13:38:23 -0700 Subject: [PATCH 58/96] Reverted Drive code to master --- src/main/java/frc4388/robot/Constants.java | 18 +--- .../java/frc4388/robot/RobotContainer.java | 12 --- .../robot/commands/DriveAtVelocityPID.java | 52 --------- .../robot/commands/DriveToDistanceMM.java | 57 ---------- .../robot/commands/DriveToDistancePID.java | 60 ----------- .../java/frc4388/robot/subsystems/Drive.java | 101 +----------------- 6 files changed, 6 insertions(+), 294 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistancePID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4ae4474..0e904eb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,12 +25,7 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; 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 double ENCODER_TICKS_PER_REV = 2048; } @@ -38,18 +33,7 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } - public static final class ElevatorConstants { - public static final int TALON_1 = 7; - public static final int TALON_2 = 8; - /* PID Constants Elevator */ - public static final int ELEVATOR_SLOT_IDX = 0; - public static final int ELEVATOR_PID_LOOP_IDX = 1; - public static final int ELEVATOR_TIMEOUT_MS = 30; - public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0); - - public static final double ENCODER_TICKS_PER_REV = 2048; - } public static final class ShooterConstants { public static final int SHOOTER_FALCON_ID = 8; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 153c296..532edaa 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,9 +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.DriveToDistancePID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; @@ -81,15 +78,6 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); - - 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(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200)); 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 3d3e96c..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,52 +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 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.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDriveVelocityPID(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/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java deleted file mode 100644 index df7ff44..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,57 +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 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.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDriveMotionMagicPID(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 deleted file mode 100644 index 79b27e9..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ /dev/null @@ -1,60 +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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -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); - SmartDashboard.putNumber("Left Target", m_leftTarget); - SmartDashboard.putNumber("Right Target", m_rightTarget); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDrivePositionPID(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 9ccb252..ef20818 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,8 +34,6 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - public static Gains m_driveGains = DriveConstants.DRIVE_GAINS; - /** * Add your docs here. */ @@ -53,63 +51,17 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Brake); + setDriveTrainNeutralMode(NeutralMode.Coast); m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed /* 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); - - /* 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); - - /* 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(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); - 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 @@ -118,23 +70,9 @@ 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(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)); - - m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); - - setDriveTrainGains(); - } catch (Exception e) { - - System.err.println("The programming team has failed successfully in the Drive Subsystem."); - + System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic."); + e.printStackTrace(System.err); } } @@ -149,23 +87,6 @@ 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_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.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_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - } - /** * Add your docs here. */ @@ -173,18 +94,6 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) { - talon.set(TalonFXControlMode.Position, targetPos); - } - - public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) { - talon.set(TalonFXControlMode.Velocity, targetVel); - } - - public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){ - talon.set(TalonFXControlMode.MotionMagic, targetPos); - } - public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); From e9122984d0a221bcda0e52d18f64df4ccdacfc81 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 1 Feb 2020 13:48:52 -0700 Subject: [PATCH 59/96] 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 7bb3b8b18d947dbe0eb234bdb3f0654aaf04a56e Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 1 Feb 2020 14:21:23 -0700 Subject: [PATCH 60/96] Set idle speed --- 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 153c296..5815b1a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,7 @@ 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)); - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); } /** From 63fae30f1dc1a7fe59f605e01d99f69d1102e9c9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 1 Feb 2020 15:30:41 -0700 Subject: [PATCH 61/96] 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 0f5a6ef09ff1ee553e37392f6f1d065dcb9a656a Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 1 Feb 2020 16:07:59 -0700 Subject: [PATCH 62/96] PID tuning P and F --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Shooter.java | 16 ++++++++-------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0e904eb..e0c6f24 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,7 +42,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.0542, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.13, 0.0, 0.0, 0.053, 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 0025383..fea891a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,7 +79,7 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index fdbc39e..3c937b2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,8 +21,6 @@ public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - private double m_targetVel = 2300; - public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; double velP; @@ -43,14 +41,11 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - - SmartDashboard.putNumber("Shooter Velocity Target", m_targetVel); } @Override public void periodic() { // This method will be called once per scheduler run - m_targetVel = SmartDashboard.getNumber("Shooter Velocity Target", m_targetVel); SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } @@ -79,10 +74,15 @@ public class Shooter extends SubsystemBase { */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { velP = actualVel/targetVel; - double runSpeed = actualVel + (20000*velP); + double runSpeed = actualVel + (12000*velP); if (runSpeed > targetVel) {runSpeed = targetVel;} - System.err.println(velP + " " + runSpeed); SmartDashboard.putNumber("shoot", actualVel); - m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + + if (actualVel < targetVel - 7000){ + m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + } + else{ + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); + } } } \ No newline at end of file From 3a59650611d0f6c4639ccfe675aca9bffa48f940 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 1 Feb 2020 16:10:39 -0700 Subject: [PATCH 63/96] 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 cfee098d057e63662465da2a4b6c5627479cbdcd Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 3 Feb 2020 16:40:39 -0700 Subject: [PATCH 64/96] Revert "Reverted Drive code to master" This reverts commit 4dd70bf1fe4fe9156c4883f7b248151f8fe50f42. --- src/main/java/frc4388/robot/Constants.java | 18 +++- .../java/frc4388/robot/RobotContainer.java | 12 +++ .../robot/commands/DriveAtVelocityPID.java | 52 +++++++++ .../robot/commands/DriveToDistanceMM.java | 57 ++++++++++ .../robot/commands/DriveToDistancePID.java | 60 +++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 101 +++++++++++++++++- 6 files changed, 294 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java create mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java create mode 100644 src/main/java/frc4388/robot/commands/DriveToDistancePID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e0c6f24..c5de68c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,7 +25,12 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; 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 double ENCODER_TICKS_PER_REV = 2048; } @@ -33,7 +38,18 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } + public static final class ElevatorConstants { + public static final int TALON_1 = 7; + public static final int TALON_2 = 8; + /* PID Constants Elevator */ + public static final int ELEVATOR_SLOT_IDX = 0; + public static final int ELEVATOR_PID_LOOP_IDX = 1; + public static final int ELEVATOR_TIMEOUT_MS = 30; + public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; + } public static final class ShooterConstants { public static final int SHOOTER_FALCON_ID = 8; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fea891a..9c5eb38 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,9 @@ 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.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; @@ -78,6 +81,15 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); + + 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(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); 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..3d3e96c --- /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.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDriveVelocityPID(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/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java new file mode 100644 index 0000000..df7ff44 --- /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.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDriveMotionMagicPID(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 new file mode 100644 index 0000000..79b27e9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* 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.wpilibj.smartdashboard.SmartDashboard; +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); + SmartDashboard.putNumber("Left Target", m_leftTarget); + SmartDashboard.putNumber("Right Target", m_rightTarget); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget); + m_drive.runDrivePositionPID(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 ef20818..9ccb252 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,6 +34,8 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public static Gains m_driveGains = DriveConstants.DRIVE_GAINS; + /** * Add your docs here. */ @@ -51,17 +53,63 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Coast); + setDriveTrainNeutralMode(NeutralMode.Brake); m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //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(true); + m_rightFrontMotor.setInverted(false); m_leftBackMotor.setInverted(InvertType.FollowMaster); 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.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); + + /* 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(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); + 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 @@ -70,9 +118,23 @@ 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(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)); + + m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); + m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); + m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); + m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + setDriveTrainGains(); + } catch (Exception e) { - System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic."); - e.printStackTrace(System.err); + + System.err.println("The programming team has failed successfully in the Drive Subsystem."); + } } @@ -87,6 +149,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_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.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_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); + } + /** * Add your docs here. */ @@ -94,6 +173,18 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } + public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) { + talon.set(TalonFXControlMode.Position, targetPos); + } + + public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) { + talon.set(TalonFXControlMode.Velocity, targetVel); + } + + public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){ + talon.set(TalonFXControlMode.MotionMagic, targetPos); + } + public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); From a8a0f60479a183f0994ec63f673d44cae7e8fc55 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 3 Feb 2020 17:31:09 -0700 Subject: [PATCH 65/96] 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 66/96] 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 67/96] 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 9d8f047bedada579f1dd01f8aaf1e77a8e524ad3 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 4 Feb 2020 18:09:40 -0700 Subject: [PATCH 68/96] Fix Shooter --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Shooter.java | 16 +++++++++------- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c5de68c..469fcb8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.13, 0.0, 0.0, 0.053, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.425, 0.0005, 13, 0.05, 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 9c5eb38..8831be9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -91,7 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 3c937b2..2e42c11 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -35,6 +35,8 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setInverted(true); setShooterGains(); + + m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -73,16 +75,16 @@ public class Shooter extends SubsystemBase { * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { - velP = actualVel/targetVel; - double runSpeed = actualVel + (12000*velP); - if (runSpeed > targetVel) {runSpeed = targetVel;} + velP = actualVel/targetVel; //Percent of target + double runSpeed = actualVel + (12000*velP); //Ramp up equation + //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); - - if (actualVel < targetVel - 7000){ - m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } else{ - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } } \ No newline at end of file From 987a3a26472bec7bd0427c2525bfe85c1646042b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 18:36:04 -0700 Subject: [PATCH 69/96] 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 70/96] 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 c0ead80055877053bfa8f08af4499c552eae5e08 Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Thu, 6 Feb 2020 19:51:17 -0700 Subject: [PATCH 71/96] Added Leveler --- src/main/java/frc4388/robot/Constants.java | 3 ++ .../java/frc4388/robot/RobotContainer.java | 5 ++ .../commands/RunLevelerWithJoystick.java | 53 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 1 + .../frc4388/robot/subsystems/Leveler.java | 44 +++++++++++++++ 5 files changed, 106 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java create mode 100644 src/main/java/frc4388/robot/subsystems/Leveler.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 793ce5c..916ada6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,6 +29,9 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 1; } + public static final class LevelerConstants { + public static final int LEVELER_TALON_ID = 9; + } 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 4131d4b..e2fddb7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -17,9 +17,11 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Leveler; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -36,6 +38,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 Leveler m_robotLeveler = new Leveler(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -54,6 +57,8 @@ 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)); + // drives the robot with an axis input from the driver controller + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); } /** diff --git a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java new file mode 100644 index 0000000..0b91068 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Leveler; +import frc4388.utility.controller.IHandController; + +public class RunLevelerWithJoystick extends CommandBase { + private Leveler m_leveler; + private IHandController m_controller; + + /** + * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller. + * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunLevelerWithJoystick(Leveler subsystem, IHandController controller) { + m_leveler = subsystem; + m_controller = controller; + addRequirements(m_leveler); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double input = m_controller.getLeftXAxis(); + m_leveler.runLeveler(input); + } + + // 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/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index ea2ba9c..a1c1a4c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -18,6 +18,7 @@ public class Intake extends SubsystemBase { * Creates a new Intake. */ public Intake() { + m_intakeMotor.setInverted(false); } diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java new file mode 100644 index 0000000..613a8cb --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* 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.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.LevelerConstants; + +public class Leveler extends SubsystemBase { + WPI_TalonSRX m_levelerMotor = new WPI_TalonSRX(LevelerConstants.LEVELER_TALON_ID); + + /** + * Creates a new Leveler. + */ + public Leveler() { + m_levelerMotor.configFactoryDefault(); + + m_levelerMotor.setNeutralMode(NeutralMode.Brake); + + m_levelerMotor.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs intake motor + * @param input the percent output to run motor at + */ + public void runLeveler(double input) { + m_levelerMotor.set(input); + } +} From f5106f42329bbed41006eaee2cf3c78f6b545485 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 6 Feb 2020 19:57:25 -0700 Subject: [PATCH 72/96] 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 73/96] 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 74/96] 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 75/96] 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 76/96] 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 43d9e8aa992cc5080385bee61ac08e49305ae11f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 7 Feb 2020 20:03:13 -0700 Subject: [PATCH 77/96] Make Position PID workie Co-Authored-By: Keenan D. Buckley --- .../java/frc4388/robot/RobotContainer.java | 25 +++++++++---------- .../commands/DriveStraightToPositionPID.java | 21 +++++++++++----- .../java/frc4388/robot/subsystems/Drive.java | 6 ++--- 3 files changed, 30 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d9c82a3..9fb7762 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; +import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; @@ -65,16 +66,8 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - //new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - // .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); - - /* Operator Buttons */ - // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - - /* PID Test Command */ + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144)); // runs velocity PID while driving straight new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) @@ -82,12 +75,18 @@ public class RobotContainer { // 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)); + // turn 45 degrees + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive)); // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + /* Operator Buttons */ + // activates "Lit Mode" + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) + .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } /** diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 67d78b3..0b9b998 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -14,8 +14,10 @@ import frc4388.robot.subsystems.Drive; public class DriveStraightToPositionPID extends CommandBase { Drive m_drive; - double m_targetPos; + double m_targetPosIn; + double m_targetPosOut; double m_targetGyro; + int i; /** * Creates a new DriveToDistancePID. @@ -25,22 +27,27 @@ public class DriveStraightToPositionPID extends CommandBase { public DriveStraightToPositionPID(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH; addRequirements(m_drive); - SmartDashboard.putNumber("Distance Target Inches", targetPos); + //SmartDashboard.putNumber("Distance Target Inches", targetPos); } // Called when the command is initially scheduled. @Override public void initialize() { + //System.err.println("PID START \n | \n |"); m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY); + i = 0; } // 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.runDriveStraightPositionPID(m_targetPos, m_targetGyro); + //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); + //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); + m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro); } // Called once the command ends or is interrupted. @@ -51,9 +58,11 @@ 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_targetPos) < 500){ + if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){ return true; } else { + i++; + //System.err.println(i); return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index ee725ad..faf161e 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -130,8 +130,8 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Diff Signal */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, @@ -329,7 +329,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - targetPos *= 2; + targetPos *= 1; m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); From 148ea4e941312b1a11060a72a408e4d0ee7d9500 Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Fri, 7 Feb 2020 21:49:04 -0700 Subject: [PATCH 78/96] Add Motion Magic (Aux PID acting up) - Aux PID drifting and causing robot to slam into walls (Aux PID works everywhere else) --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 3 +- .../commands/DriveStraightToPositionMM.java | 70 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 21 ++++-- 4 files changed, 88 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 27d5359..f4b73dc 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -32,8 +32,8 @@ public final class Constants { 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; + public static final int DRIVE_CRUISE_VELOCITY = 20000; + public static final int DRIVE_ACCELERATION = 7000; /* Remote Sensors */ public final static int REMOTE_0 = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9fb7762..f343dbb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; +import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -74,7 +75,7 @@ public class RobotContainer { .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)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 400)); // turn 45 degrees new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java new file mode 100644 index 0000000..69bf200 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* 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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.subsystems.Drive; + +public class DriveStraightToPositionMM extends CommandBase { + Drive m_drive; + double m_targetPosIn; + double m_targetPosOut; + double m_targetGyro; + boolean isGoneFast; + + /** + * Creates a new DriveToDistancePID. + * @param subsystem drive subsystem + * @param targetPos distance to travel in inches + */ + public DriveStraightToPositionMM(Drive subsystem, double targetPos) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH; + addRequirements(m_drive); + //SmartDashboard.putNumber("Distance Target Inches", targetPos); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.err.println("PID START \n | \n |"); + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY); + isGoneFast = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); + System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + m_drive.runMotionMagicPID(m_targetPosOut, 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() { + if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){ + return true; + } else { + if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) { + isGoneFast = true; + } + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index faf161e..2f4c2e0 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; @@ -100,6 +101,15 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + /* 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); @@ -321,7 +331,7 @@ public class Drive extends SubsystemBase { } /** - * Runs a position PID while driving straight (has not been tested) + * Runs a position PID while driving straight * @param targetPos The position to drive to in units * @param targetGyro The angle to drive at in units */ @@ -329,7 +339,6 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - targetPos *= 1; m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); @@ -344,8 +353,6 @@ public class Drive extends SubsystemBase { 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.AuxOutput1); @@ -360,10 +367,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_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - + m_driveTrain.feedWatchdog(); } From 71bd46cc8518c4e9bf3bd70d2213098d21f52f79 Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Sat, 8 Feb 2020 09:27:40 -0700 Subject: [PATCH 79/96] Tested Motion Magic and commented out unnecessary data prints --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/DriveStraightToPositionMM.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f343dbb..2e159df 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -75,7 +75,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 400)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72)); // turn 45 degrees new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 69bf200..259c571 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -44,9 +44,9 @@ public class DriveStraightToPositionMM extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); + //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); } From ea632f626fa95253d1df602b2a24e2b890004e50 Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Sat, 8 Feb 2020 12:37:48 -0700 Subject: [PATCH 80/96] Added DriveWithJoystickAuxPID Has been tested, does work but it stalled a falcon. --- .../java/frc4388/robot/RobotContainer.java | 4 + .../robot/commands/DriveToDistanceMM.java | 60 -------------- .../commands/DriveWithJoystickAuxPID.java | 79 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 9 +++ 4 files changed, 92 insertions(+), 60 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java create mode 100644 src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2e159df..9e4f64d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.DriveWithJoystickAuxPID; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; @@ -73,6 +74,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72)); diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java deleted file mode 100644 index 625e522..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,60 +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 edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -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. - * @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 * DriveConstants.TICKS_PER_INCH; - 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/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java new file mode 100644 index 0000000..2531847 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* 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.TalonFXControlMode; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpiutil.math.MathUtil; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.subsystems.Drive; +import frc4388.utility.controller.IHandController; + +public class DriveWithJoystickAuxPID extends CommandBase { + Drive m_drive; + double m_targetGyro; + long lastTime; + IHandController m_controller; + + /** + * Creates a new DriveWithJoystickAuxPID. + */ + public DriveWithJoystickAuxPID(Drive subsystem, IHandController controller) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_controller = controller; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + lastTime = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + double moveInput = m_controller.getLeftYAxis(); + double steerInput = m_controller.getRightXAxis(); + double moveOutput = 0; + long deltaTime = System.currentTimeMillis() - lastTime; + lastTime = System.currentTimeMillis(); + if (moveInput >= 0){ + moveOutput = -Math.cos(1.571*moveInput)+1; + } else { + moveOutput = Math.cos(1.571*moveInput)-1; + } + + m_targetGyro += 2 * steerInput * deltaTime; + + m_targetGyro = MathUtil.clamp(m_targetGyro, + currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), + currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); + + m_drive.driveWithInputAux(moveOutput, m_targetGyro); + + System.err.println("Target: " + m_targetGyro); + System.err.println("Current: " + currentGyro); + System.err.println(); + } + + // 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/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2f4c2e0..e629ab8 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -330,6 +330,15 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } + public void driveWithInputAux(double move, double targetGyro) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + + m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro); + m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + + m_driveTrain.feedWatchdog(); + } + /** * Runs a position PID while driving straight * @param targetPos The position to drive to in units From 8cbd39ebe8deff37f11e301a2031b6a9ab1259cf Mon Sep 17 00:00:00 2001 From: keenandbuckley Date: Sat, 8 Feb 2020 13:01:25 -0700 Subject: [PATCH 81/96] Added double solenoid and method to shift gears Has not been tested yet, waiting for solenoids to arrive. --- .../java/frc4388/robot/RobotContainer.java | 8 ++++++++ .../java/frc4388/robot/subsystems/Drive.java | 18 ++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d9c82a3..0ad14ab 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -83,6 +83,14 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); + // sets solenoids into high gear + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + // sets solenoids into low gear + new JoystickButton(getDriverJoystick(), XboxController.LEFT_TRIGGER_AXIS) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); // interrupts any running command diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index ee725ad..52f3fa6 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -20,6 +20,7 @@ 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.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -49,6 +50,8 @@ public class Drive extends SubsystemBase { public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + public DoubleSolenoid speedShift; + /** * Add your docs here. */ @@ -61,6 +64,8 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); + speedShift = new DoubleSolenoid(1,0,1); + /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); @@ -414,4 +419,17 @@ public class Drive extends SubsystemBase { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); } + + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + speedShift.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + speedShift.set(DoubleSolenoid.Value.kReverse); + } + } } 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 82/96] 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 83/96] 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)); } From 5ab2fce79bb2e4e4d7c4633e85d532c214726b16 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 14:49:16 -0700 Subject: [PATCH 84/96] Set up leveler motor so that it can be run. --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Leveler.java | 11 ++++++----- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e9c899c..e788406 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,11 +70,11 @@ public final class Constants { } public static final class LevelerConstants { - public static final int LEVELER_TALON_ID = 9; + public static final int LEVELER_CAN_ID = 9; } public static final class StorageConstants { - public static final int STORAGE_CAN_ID = 9; + public static final int STORAGE_CAN_ID = -1; 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e7ae628..cf5b717 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,7 @@ 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)); - // drives the robot with an axis input from the driver controller + // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // runs storage motor at 50 percent m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index 613a8cb..02df406 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -10,22 +10,23 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LevelerConstants; public class Leveler extends SubsystemBase { - WPI_TalonSRX m_levelerMotor = new WPI_TalonSRX(LevelerConstants.LEVELER_TALON_ID); + CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); /** * Creates a new Leveler. */ public Leveler() { - m_levelerMotor.configFactoryDefault(); - - m_levelerMotor.setNeutralMode(NeutralMode.Brake); - + m_levelerMotor.restoreFactoryDefaults(); + m_levelerMotor.setIdleMode(IdleMode.kCoast); m_levelerMotor.setInverted(false); } From 0015b078cbcf45f0380e24931ecce996a1cd0df0 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:11:23 -0700 Subject: [PATCH 85/96] Turned off limit switches so climber would run. --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Climber.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 50bbe8a..4ca2bc8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,7 @@ public class RobotContainer { 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())); - // drives motor with input from triggers on the opperator controller + // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index de0b618..6b35036 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -27,8 +27,8 @@ public class Climber extends SubsystemBase { m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_forwardLimit.enableLimitSwitch(true); - m_reverseLimit.enableLimitSwitch(true); + m_forwardLimit.enableLimitSwitch(false); + m_reverseLimit.enableLimitSwitch(false); } @Override From 92feb32651727881c1df41dc80706c09c5497ce3 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:23:42 -0700 Subject: [PATCH 86/96] Reconfigured the motor to get intake to run. --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Intake.java | 5 ++++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e788406..1904d8d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,11 +66,11 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = 1; + public static final int INTAKE_SPARK_ID = 9; } public static final class LevelerConstants { - public static final int LEVELER_CAN_ID = 9; + public static final int LEVELER_CAN_ID = -1; } public static final class StorageConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cf5b717..5387b64 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,7 +56,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())); - // drives motor with input from triggers on the opperator controller + // drives intake 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 m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index a1c1a4c..09d1827 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,12 +7,15 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { - Spark m_intakeMotor = new Spark(IntakeConstants.INTAKE_SPARK_ID); + CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); /** * Creates a new Intake. From 9aeac07431f05023d6e386ddfe9ed86a4baf0350 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:58:11 -0700 Subject: [PATCH 87/96] Added RunExtenderOutIn command and defined extender motor --- src/main/java/frc4388/robot/Constants.java | 1 + .../robot/commands/RunExtenderOutIn.java | 59 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 28 ++++++++- 3 files changed, 86 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunExtenderOutIn.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c3640d2..2840cdb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,6 +67,7 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 9; + public static final int EXTENDER_SPARK_ID = 10; } public static final class ClimberConstants { diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java new file mode 100644 index 0000000..d5a2e29 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Intake; +import frc4388.utility.controller.IHandController; + +public class RunExtenderOutIn extends CommandBase { + private Intake m_intake; + private boolean isOut = false; + + /** + * Uses input from opperator triggers to control intake motor. + * The right trigger will run the intake in and the left trigger will run it out. + * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunExtenderOutIn(Intake subsystem, IHandController controller) { + // Use addRequirements() here to declare subsystem dependencies. + m_intake = subsystem; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + isOut = !isOut; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (isOut){ + m_intake.runExtender(0.3); + } else { + m_intake.runExtender(-0.3); + } + + } + + // 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/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 09d1827..62bb30a 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,7 +7,10 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANDigitalInput; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Spark; @@ -16,13 +19,26 @@ import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); - + CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + /** * Creates a new Intake. */ public Intake() { - + m_intakeMotor.restoreFactoryDefaults(); + m_extenderMotor.restoreFactoryDefaults(); + + m_intakeMotor.setIdleMode(IdleMode.kCoast); + m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); + m_extenderMotor.setInverted(false); + + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderForwardLimit.enableLimitSwitch(false); + m_extenderReverseLimit.enableLimitSwitch(false); } @Override @@ -37,4 +53,12 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(input); } + + /** + * Runs extender motor + * @param input the percent output to run motor at + */ + public void runExtender(double input) { + m_extenderMotor.set(input); + } } From 827aca7fce5ddf4821e000d92fd118295070ad10 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 8 Feb 2020 16:10:21 -0700 Subject: [PATCH 88/96] Small edits to make it work --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Drive.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0ad14ab..c4cc293 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -84,11 +84,11 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.LEFT_TRIGGER_AXIS) + new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 52f3fa6..07ba57a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -64,7 +64,7 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); - speedShift = new DoubleSolenoid(1,0,1); + speedShift = new DoubleSolenoid(7,0,1); /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); From 674523687939b011f3aa651914dae69bfa44354b Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Mon, 10 Feb 2020 17:37:26 -0700 Subject: [PATCH 89/96] Added an intake Extender command to be tested. --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++++ .../java/frc4388/robot/commands/RunExtenderOutIn.java | 11 +++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6accd17..8ecddc4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; +import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; @@ -87,6 +88,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(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(new RunExtenderOutIn(m_robotIntake)); /* PID Test Command */ // runs velocity PID while driving straight diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index d5a2e29..c9e6de9 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -14,16 +14,14 @@ import frc4388.utility.controller.IHandController; public class RunExtenderOutIn extends CommandBase { private Intake m_intake; private boolean isOut = false; + private long startTime; /** * Uses input from opperator triggers to control intake motor. * The right trigger will run the intake in and the left trigger will run it out. * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ - public RunExtenderOutIn(Intake subsystem, IHandController controller) { + public RunExtenderOutIn(Intake subsystem) { // Use addRequirements() here to declare subsystem dependencies. m_intake = subsystem; addRequirements(m_intake); @@ -33,6 +31,7 @@ public class RunExtenderOutIn extends CommandBase { @Override public void initialize() { isOut = !isOut; + startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @@ -49,11 +48,15 @@ public class RunExtenderOutIn extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_intake.runExtender(0.0); } // Returns true when the command should end. @Override public boolean isFinished() { + if (startTime + 3000 < System.currentTimeMillis()) { + return true; + } return false; } } From 80a0198965d70ee00670cd51de0f1fc7d8dce5a4 Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Mon, 10 Feb 2020 17:50:52 -0700 Subject: [PATCH 90/96] Updated comments to be accurate --- src/main/java/frc4388/robot/commands/RunExtenderOutIn.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index c9e6de9..b0bb140 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -17,8 +17,8 @@ public class RunExtenderOutIn extends CommandBase { private long startTime; /** - * Uses input from opperator triggers to control intake motor. - * The right trigger will run the intake in and the left trigger will run it out. + * Uses input from opperator to run the extender motor. + * The left bumper will run the extender in and out. * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ public RunExtenderOutIn(Intake subsystem) { From 07ab93a06fd4312924ada6b123c925ed39830ffa Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 10 Feb 2020 16:57:41 -0800 Subject: [PATCH 91/96] Added Neo PID stuff in Storage --- src/main/java/frc4388/robot/Constants.java | 17 ++++++++++++ src/main/java/frc4388/robot/Gains.java | 2 ++ .../frc4388/robot/subsystems/Storage.java | 26 ++++++++++++++++--- 3 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f4f8c2b..bb630ff 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -79,12 +79,29 @@ public final class Constants { public static final class StorageConstants { public static final int STORAGE_CAN_ID = -1; + + /* Ball Indexes */ 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 = 4; public static final int BEAM_SENSOR_DIO_5 = 5; + + /* PID Values */ + public static final int SLOT_DISTANCE = 0; + + /* PID Indexes */ + public static final int PID_PRIMARY = 0; + + /* PID Gains */ + public static final double storP = 0.1; + public static final double storI = 1e-4; + public static final double storD = 1.0; + public static final double storIz = 0.0; + public static final double storF = 0.0; + public static final double storkmaxOutput = 1.0; + public static final double storkminOutput = -1.0; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/robot/Gains.java index b2f1de2..7d2b3d7 100644 --- a/src/main/java/frc4388/robot/Gains.java +++ b/src/main/java/frc4388/robot/Gains.java @@ -17,6 +17,8 @@ public class Gains { public double m_kF; public int m_kIzone; public double m_kPeakOutput; + public double m_kmaxOutput; + public double m_kminOutput; /** * Creates Gains object for PIDs diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index a7d1bfb..5ac0cb9 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -9,7 +9,9 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -21,6 +23,9 @@ import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); private DigitalInput[] m_beamSensors = new DigitalInput[6]; + + CANPIDController m_storagePIDController = m_storageMotor.getPIDController(); + /** * Creates a new Storage. */ @@ -40,17 +45,32 @@ public class Storage extends SubsystemBase { /** * Runs storage motor + * * @param input the voltage to run motor at */ - public void runStorage(double input) { + public void runStorage(final double input) { m_storageMotor.set(input); - boolean beam_on = m_beamSensors[0].get(); + final boolean beam_on = m_beamSensors[0].get(); if (beam_on) { System.err.println("Beam on"); } else { System.err.println("Beam off"); } - + + } + + /* Storage PID Control */ + public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + { + // Set PID Coefficients + m_storagePIDController.setP(kP); + m_storagePIDController.setI(kI); + m_storagePIDController.setD(kD); + m_storagePIDController.setIZone(kIz); + m_storagePIDController.setFF(kF); + m_storagePIDController.setOutputRange(kminOutput, kmaxOutput); + + m_storagePIDController.setReference(targetPos, ControlType.kPosition); } } From 6fbbca937c18083b65208419d238f4eb0101ff6d Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 10 Feb 2020 18:02:37 -0700 Subject: [PATCH 92/96] fix keenans dumb merge --- .../java/frc4388/robot/RobotContainer.java | 3 - .../robot/commands/DriveAtVelocityPID.java | 52 ---------------- .../robot/commands/DriveToDistanceMM.java | 60 ------------------- .../robot/commands/DriveToDistancePID.java | 60 ------------------- .../java/frc4388/robot/subsystems/Drive.java | 17 ------ .../frc4388/robot/subsystems/Shooter.java | 8 +-- 6 files changed, 4 insertions(+), 196 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java delete mode 100644 src/main/java/frc4388/robot/commands/DriveToDistanceMM.java delete 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 bfbbac0..355708b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,9 +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.DriveToDistancePID; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; 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 3d3e96c..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,52 +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 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.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDriveVelocityPID(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/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java deleted file mode 100644 index 625e522..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,60 +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 edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -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. - * @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 * DriveConstants.TICKS_PER_INCH; - 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 deleted file mode 100644 index 79b27e9..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ /dev/null @@ -1,60 +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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -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); - SmartDashboard.putNumber("Left Target", m_leftTarget); - SmartDashboard.putNumber("Right Target", m_rightTarget); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDrivePositionPID(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 26ad512..35db61c 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -318,23 +318,6 @@ 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_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.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_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - } - /** * Add your docs here. */ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 2e42c11..7c90fb3 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -64,10 +64,10 @@ public class Shooter extends SubsystemBase { */ public void setShooterGains() { m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } /** * Runs drum shooter velocity PID. From 952649e21bee83dc78548b99d8c0c3ebaae10dbd Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 10 Feb 2020 17:06:17 -0800 Subject: [PATCH 93/96] Added command to run PID method to Robot Container --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4ca2bc8..0e771f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -102,6 +102,10 @@ public class RobotContainer { // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + /* Storage Neo PID Test */ + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whenPressed(new InstantCommand(() -> m_robotStorage.runStoragePositionPID(20, 0.1, 1e-4, 1.0, 0.0, 0.0, 1, -1))); } /** From 82ebcef450096df08e69d5d4a1c10e7cbf90fcf9 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 10 Feb 2020 19:21:10 -0700 Subject: [PATCH 94/96] Tuning and Comments --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/subsystems/Shooter.java | 6 +++--- 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 d7b76af..ef8356a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -76,7 +76,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.425, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 7c90fb3..169e36f 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -25,7 +25,7 @@ public class Shooter extends SubsystemBase { double velP; /** - * Creates a new Shooter. + * Creates a new Shooter subsystem. */ public Shooter() { m_shooterFalcon.configFactoryDefault(); @@ -80,10 +80,10 @@ public class Shooter extends SubsystemBase { //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); runSpeed = runSpeed/targetVel; //Convert to percent - if (actualVel < targetVel - 1000){ + if (actualVel < targetVel - 1000){ //PID Based on ramp up amount m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } - else{ + else{ //PID Based on targetVel m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } From 1b63e2a63d3617e2a5759332166ed9ad44594711 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 11 Feb 2020 02:56:55 +0000 Subject: [PATCH 95/96] Update RobotContainer.java --- 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 bdff586..285d467 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,7 +101,7 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); From a4ac866d1c207536ff86f5cf976595792e97ec9f Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 10 Feb 2020 19:13:34 -0800 Subject: [PATCH 96/96] Tested and working Neo PID --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/Storage.java | 16 ++++++++++++++++ 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bb630ff..26a3feb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -78,7 +78,7 @@ public final class Constants { } public static final class StorageConstants { - public static final int STORAGE_CAN_ID = -1; + public static final int STORAGE_CAN_ID = 10; /* Ball Indexes */ public static final int BEAM_SENSOR_DIO_0 = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0e771f5..63cfd7a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotStorage.runStoragePositionPID(20, 0.1, 1e-4, 1.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5ac0cb9..84f01ec 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -9,6 +9,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; @@ -16,6 +17,7 @@ import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.StorageConstants; @@ -26,10 +28,14 @@ public class Storage extends SubsystemBase { CANPIDController m_storagePIDController = m_storageMotor.getPIDController(); + CANEncoder m_encoder = m_storageMotor.getEncoder(); + /** * Creates a new Storage. */ public Storage() { + resetEncoder(); + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); @@ -60,6 +66,11 @@ public class Storage extends SubsystemBase { } + public void resetEncoder() + { + m_encoder.setPosition(0); + } + /* Storage PID Control */ public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) { @@ -73,4 +84,9 @@ public class Storage extends SubsystemBase { m_storagePIDController.setReference(targetPos, ControlType.kPosition); } + + public void getEncoderPos() + { + m_encoder.getPosition(); + } }