mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 08:48:01 -06:00
Added Odometry and Baseplate for Trajectory
Need to still add Autonomous Command for actual Trajectory.
This commit is contained in:
Aarav Shah
@@ -21,6 +21,10 @@ 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.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
@@ -49,6 +53,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 final DifferentialDriveOdometry m_odometry;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
@@ -61,6 +67,8 @@ public class Drive extends SubsystemBase {
|
||||
m_pigeon.configFactoryDefault();
|
||||
resetGyroYaw();
|
||||
|
||||
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
|
||||
|
||||
/* set back motors as followers */
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
@@ -69,7 +77,8 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/* 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_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);
|
||||
@@ -78,148 +87,154 @@ public class Drive extends SubsystemBase {
|
||||
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.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.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
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.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.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);
|
||||
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);
|
||||
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
resetEncoders();
|
||||
|
||||
/* 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
|
||||
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
|
||||
|
||||
/* 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.configSelectedFeedbackSensor(
|
||||
* FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
* DriveConstants.PID_PRIMARY, // PID Index for Source [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);
|
||||
/*
|
||||
* 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
|
||||
|
||||
/*
|
||||
* 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);
|
||||
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.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);
|
||||
|
||||
/* 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
|
||||
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
|
||||
DriveConstants.PID_TURN,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY,
|
||||
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, // 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_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_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* 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);
|
||||
|
||||
/* Smart Dashboard Initial Values */
|
||||
|
||||
/* Set up Chooser */
|
||||
/* Set up Chooser */
|
||||
m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
|
||||
//setDriveTrainGains("Distance PID", m_gainsDistance);
|
||||
// setDriveTrainGains("Distance PID", m_gainsDistance);
|
||||
m_chooser.addOption("Velocity PID", m_gainsVelocity);
|
||||
//setDriveTrainGains("Velocity PID", m_gainsVelocity);
|
||||
// setDriveTrainGains("Velocity PID", m_gainsVelocity);
|
||||
m_chooser.addOption("Turning PID", m_gainsTurning);
|
||||
//setDriveTrainGains("Turning PID", m_gainsTurning);
|
||||
// setDriveTrainGains("Turning PID", m_gainsTurning);
|
||||
m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
|
||||
//setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
|
||||
// setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
|
||||
Shuffleboard.getTab("PID").add(m_chooser);
|
||||
|
||||
/* Gyro */
|
||||
/* Gyro */
|
||||
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
|
||||
/* Sensor Values */
|
||||
/* 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());
|
||||
|
||||
/* PID */
|
||||
/* PID */
|
||||
Gains gains = m_chooser.getSelected();
|
||||
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);
|
||||
|
||||
|
||||
/**
|
||||
* 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);
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
* 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_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_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
|
||||
*/
|
||||
* 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);
|
||||
}
|
||||
|
||||
@@ -234,8 +249,10 @@ public class Drive extends SubsystemBase {
|
||||
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 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());
|
||||
@@ -251,12 +268,18 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
} catch (Exception e) {
|
||||
System.err.println("Error in the Drive Subsystem");
|
||||
//e.printStackTrace(System.err);
|
||||
// e.printStackTrace(System.err);
|
||||
}
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees(getHeading()),
|
||||
m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(),
|
||||
m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition());
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
*
|
||||
* @param mode NeutralMode to set motors to
|
||||
*/
|
||||
public void setDriveTrainNeutralMode(NeutralMode mode) {
|
||||
@@ -268,10 +291,12 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/**
|
||||
* Initializes the drive train gains kP, kI, kD, and kF
|
||||
* @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or "Turning PID"
|
||||
*
|
||||
* @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){
|
||||
public void setDriveTrainGains(String slot, Gains gains) {
|
||||
/* Distance */
|
||||
if (slot.equals("Distance PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
@@ -288,7 +313,8 @@ public class Drive extends SubsystemBase {
|
||||
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);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
/* Turning */
|
||||
if (slot.equals("Turning PID")) {
|
||||
@@ -297,7 +323,8 @@ public class Drive extends SubsystemBase {
|
||||
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);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Motion Magic */
|
||||
@@ -307,8 +334,9 @@ public class Drive extends SubsystemBase {
|
||||
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.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
}
|
||||
@@ -316,13 +344,14 @@ public class Drive extends SubsystemBase {
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void driveWithInput(double move, double steer){
|
||||
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 targetPos The position to drive to in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
|
||||
@@ -338,7 +367,8 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/**
|
||||
* Runs velocity PID while driving straight
|
||||
* @param targetVel The velocity to drive at in units
|
||||
*
|
||||
* @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) {
|
||||
@@ -354,26 +384,28 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/**
|
||||
* Runs motion magic PID while driving straight (has not been tested)
|
||||
* @param targetPos The position to drive to in units
|
||||
*
|
||||
* @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){
|
||||
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();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a Turning PID to rotate a to a target angle
|
||||
*
|
||||
* @param targetAngle target angle in degrees
|
||||
*/
|
||||
public void runTurningPID(double targetAngle){
|
||||
double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
public void runTurningPID(double targetAngle) {
|
||||
double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
runDriveStraightVelocityPID(0, targetGyro);
|
||||
}
|
||||
|
||||
@@ -382,7 +414,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public double getGyroYaw() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[0];
|
||||
}
|
||||
@@ -392,7 +424,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public double getGyroPitch() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[1];
|
||||
}
|
||||
@@ -402,7 +434,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public double getGyroRoll() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[2];
|
||||
}
|
||||
@@ -414,4 +446,66 @@ public class Drive extends SubsystemBase {
|
||||
m_pigeon.setYaw(0);
|
||||
m_pigeon.setAccumZAngle(0);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot
|
||||
*
|
||||
* @return The robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Math.IEEEremainder(getGyroYaw(), 360);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns current wheel speeds of robot.
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
|
||||
return new DifferentialDriveWheelSpeeds( m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(),
|
||||
m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the encoders for both motors.
|
||||
*/
|
||||
public void resetEncoders() {
|
||||
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void setOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the encoder value (position) of a motor
|
||||
* @param falcon The motor to get the position of
|
||||
* @return The position of the motor in inches
|
||||
*/
|
||||
public double getDistanceInches(WPI_TalonFX falcon) {
|
||||
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in ticks to inches.
|
||||
* @param ticks The value in ticks to convert
|
||||
* @return The converted value in inches
|
||||
*/
|
||||
public double ticksToInches(double ticks) {
|
||||
return ticks * DriveConstants.INCHES_PER_TICK;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user