mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 08:48:01 -06:00
Set up back motor master and tune PIDs
This commit is contained in:
Keenan D. Buckley
@@ -30,8 +30,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.1, 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.5);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.05, 0.0, 0.0, 0.025, 0, 1.0);
|
||||
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.5);
|
||||
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 = 20000;
|
||||
public static final int DRIVE_ACCELERATION = 7000;
|
||||
|
||||
@@ -64,6 +64,7 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
//m_robotContainer.setDriveGearState(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -78,6 +79,7 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
m_robotContainer.resetOdometry();
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
@@ -105,6 +107,7 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
|
||||
@@ -102,7 +102,7 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
@@ -131,7 +131,7 @@ public class RobotContainer {
|
||||
|
||||
// turn 45 degrees
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
|
||||
|
||||
// sets solenoids into high gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
|
||||
@@ -153,11 +153,20 @@ public class RobotContainer {
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
* @param mode NeutralMode to set motors to
|
||||
* @param state the gearing of the gearbox (true is high, false is low)
|
||||
*/
|
||||
public void setDriveNeutralMode(NeutralMode mode) {
|
||||
m_robotDrive.setDriveTrainNeutralMode(mode);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the gear of the drivetrain
|
||||
* @param state the gearing of the gearbox (true is high, false is low)
|
||||
*/
|
||||
public void setDriveGearState(boolean state) {
|
||||
m_robotDrive.setShiftState(state);
|
||||
}
|
||||
|
||||
public void configDriveTrainSensors(FeedbackDevice type) {
|
||||
m_robotDrive.configMotorSensor(type);
|
||||
}
|
||||
@@ -185,11 +194,11 @@ public class RobotContainer {
|
||||
new Pose2d(0, 0, new Rotation2d(0)),
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(
|
||||
new Translation2d(2, 0)
|
||||
//new Translation2d(4, -2)
|
||||
new Translation2d(1, 1),
|
||||
new Translation2d(2, -1)
|
||||
),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(4, 0, new Rotation2d(0)),
|
||||
new Pose2d(3, 0, new Rotation2d(0)),
|
||||
// Pass config
|
||||
config);
|
||||
|
||||
|
||||
@@ -85,13 +85,6 @@ public class Drive extends SubsystemBase {
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
setDriveTrainNeutralMode(NeutralMode.Coast);
|
||||
|
||||
/* 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);
|
||||
@@ -99,6 +92,15 @@ public class Drive extends SubsystemBase {
|
||||
m_leftBackMotor.setInverted(InvertType.FollowMaster);
|
||||
m_rightBackMotor.setInverted(InvertType.FollowMaster);
|
||||
|
||||
setDriveTrainNeutralMode(NeutralMode.Coast);
|
||||
|
||||
/* 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.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
//m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
|
||||
|
||||
/* PID for Front Motor Control in Teleop */
|
||||
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);
|
||||
@@ -130,6 +132,21 @@ public class Drive extends SubsystemBase {
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* PID for Back Motor control in Auto */
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sensors for WPI_TalonFXs */
|
||||
resetEncoders();
|
||||
|
||||
@@ -138,18 +155,22 @@ public class Drive extends SubsystemBase {
|
||||
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
|
||||
*/
|
||||
/* Configure the left back Talon's selected sensor as local QuadEncoder */
|
||||
m_leftBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the right back Talon's selected sensor as local QuadEncoder */
|
||||
m_rightBackMotor.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
|
||||
|
||||
/*
|
||||
* Configure the Pigeon IMU to the other Remote Slot available on the right
|
||||
* Talon
|
||||
*/
|
||||
/* 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);
|
||||
|
||||
@@ -164,19 +185,22 @@ public class Drive extends SubsystemBase {
|
||||
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
|
||||
configMotorSensor(FeedbackDevice.SensorDifference);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
/*
|
||||
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,
|
||||
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);
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//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);
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//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);
|
||||
@@ -185,18 +209,8 @@ public class Drive extends SubsystemBase {
|
||||
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 */
|
||||
|
||||
/* 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());
|
||||
m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* Max out the peak output (for all modes). However you can limit the output of
|
||||
@@ -207,6 +221,11 @@ public class Drive extends SubsystemBase {
|
||||
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.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
|
||||
@@ -221,6 +240,15 @@ public class Drive extends SubsystemBase {
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
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
|
||||
@@ -247,16 +275,16 @@ 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());
|
||||
//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 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));
|
||||
@@ -271,8 +299,8 @@ public class Drive extends SubsystemBase {
|
||||
}
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
|
||||
inchesToMeters(-getDistanceInches(m_rightFrontMotor)));
|
||||
inchesToMeters(getDistanceInches(m_leftBackMotor)),
|
||||
-inchesToMeters(getDistanceInches(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -293,6 +321,8 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -305,6 +335,8 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
@@ -321,6 +353,8 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
@@ -334,8 +368,11 @@ public class Drive extends SubsystemBase {
|
||||
public void runDriveVelocityPID(double targetVel, double targetGyro) {
|
||||
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, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
@@ -351,6 +388,8 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
@@ -369,33 +408,38 @@ public class Drive extends SubsystemBase {
|
||||
/**
|
||||
* Controls the left and right sides of the drive with velocity targets.
|
||||
*
|
||||
* @param leftSpeed the commanded left output
|
||||
* @param rightSpeed the commanded right output
|
||||
* @param leftSpeed the commanded left speed
|
||||
* @param rightSpeed the commanded right speed
|
||||
*/
|
||||
public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
|
||||
DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
|
||||
ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
|
||||
double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
|
||||
double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
|
||||
double angleVelDeg = Math.toDegrees(angleVelRad);
|
||||
//DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
|
||||
//ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
|
||||
//double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
|
||||
//double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
|
||||
//double angleVelDeg = Math.toDegrees(angleVelRad);
|
||||
|
||||
m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
|
||||
m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle,
|
||||
m_currentAngleYaw-(360),
|
||||
m_currentAngleYaw+(360));
|
||||
double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
double moveVel = inchesToTicks(metersToInches(moveVelMPS))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
//double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
//m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
|
||||
//m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle,
|
||||
// m_currentAngleYaw-(360),
|
||||
// m_currentAngleYaw+(360));
|
||||
//double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
|
||||
//SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
|
||||
//SmartDashboard.putNumber("Move Vel Right", moveVelRight);
|
||||
|
||||
runDriveVelocityPID(moveVel*2, targetGyro);
|
||||
//runDriveVelocityPID(moveVel*2, targetGyro);
|
||||
|
||||
//m_rightFrontMotor.set(TalonFXControlMode.Velocity, moveVelRight);
|
||||
//m_leftFrontMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
|
||||
//m_driveTrain.feedWatchdog();
|
||||
m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight);
|
||||
m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
|
||||
m_leftFrontMotor.follow(m_leftFrontMotor);
|
||||
m_rightFrontMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -486,8 +530,8 @@ public class Drive extends SubsystemBase {
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
|
||||
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftFrontMotor)),
|
||||
inchesToMeters(getVelocityInchesPerSecond(m_rightFrontMotor)));
|
||||
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)),
|
||||
-inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -496,6 +540,8 @@ public class Drive extends SubsystemBase {
|
||||
public void resetEncoders() {
|
||||
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user