mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
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.
This commit is contained in:
aarav18
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user