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

Add PID and Motion Magic Commands

Browse Source
This commit is contained in:
Aarav
2020-01-16 20:22:50 -07:00
parent 6fc414ccb8
commit 9be99be0c3
4 changed files with 71 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+8 -4
View File
@@ -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);