Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Added Odometry and Baseplate for Trajectory

Browse Source 
Need to still add Autonomous Command for actual Trajectory.
This commit is contained in:
Aarav Shah
2020-02-10 20:13:15 -07:00
parent 68a39f8eca
commit 27824bdcfc
3 changed files with 202 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -50,16 +50,16 @@ public final class Constants {
public final static int SLOT_MOTION_MAGIC = 3;
/* Drive Train Characteristics */
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 TICKS_PER_MOTOR_REV = 2048;
public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
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;
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 WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
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;
src/main/java/frc4388/robot/RobotContainer.java
+2
View File
@@ -10,6 +10,8 @@ package frc4388.robot;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
src/main/java/frc4388/robot/subsystems/Drive.java
+196 -102
View File
@@ -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;
}
}