mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Velocity Control PID
Does not work in testing
This commit is contained in:
aarav18
@@ -52,7 +52,7 @@ public class RobotContainer {
|
||||
|
||||
/* Default Commands */
|
||||
// drives the robot with a two-axis input from the driver controller
|
||||
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
|
||||
//m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
|
||||
// drives motor with input from triggers on the opperator controller
|
||||
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
@@ -79,7 +79,7 @@ public class RobotContainer {
|
||||
|
||||
/* PID Test Command */
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
|
||||
.whileHeld(new RunCommand(() -> m_robotDrive.runVelocityPID(2000), m_robotDrive));
|
||||
.whileHeld(new DriveAtVelocityPID(m_robotDrive, 50));
|
||||
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
|
||||
|
||||
@@ -9,6 +9,7 @@ package frc4388.robot.commands;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FollowerType;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
@@ -18,6 +19,7 @@ public class DriveAtVelocityPID extends CommandBase {
|
||||
double m_targetVel;
|
||||
double m_leftTarget;
|
||||
double m_rightTarget;
|
||||
double m_copiedTargetVel;
|
||||
/**
|
||||
* Creates a new DriveAtVelocityPID.
|
||||
* @param subsystem drive subsystem
|
||||
@@ -26,23 +28,27 @@ public class DriveAtVelocityPID extends CommandBase {
|
||||
public DriveAtVelocityPID(Drive subsystem, double targetVel) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_targetVel = targetVel /* DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;*/;
|
||||
m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
m_copiedTargetVel = targetVel;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_leftTarget = -m_targetVel;
|
||||
m_leftTarget = m_targetVel;
|
||||
m_rightTarget = m_targetVel;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_drive.runVelocityPID(m_rightTarget);
|
||||
m_drive.runVelocityPID(m_leftTarget);
|
||||
m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput);
|
||||
m_drive.runVelocityPID(m_targetVel);
|
||||
|
||||
SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel);
|
||||
SmartDashboard.putNumber("Output Target Velocity", m_targetVel);
|
||||
//m_drive.runVelocityPID(m_leftTarget);
|
||||
//m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -75,42 +75,45 @@ public class Drive extends SubsystemBase {
|
||||
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
|
||||
/*m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
|
||||
RemoteSensorSource.TalonSRX_SelectedSensor, // Remote Feedback Source
|
||||
DriveConstants.REMOTE_0, // Source number [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
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(),
|
||||
/*m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(),
|
||||
RemoteSensorSource.Pigeon_Yaw,
|
||||
DriveConstants.REMOTE_1,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
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.SensorSum,
|
||||
DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum,
|
||||
// DriveConstants.PID_PRIMARY,
|
||||
// DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Scale Feedback by 0.5 to half the sum of Distance */
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient
|
||||
DriveConstants.PID_PRIMARY, // PID Slot of Source
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
//m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient
|
||||
// DriveConstants.PID_PRIMARY, // PID Slot of Source
|
||||
// DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* 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,
|
||||
// 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);
|
||||
//m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0,
|
||||
// DriveConstants.PID_PRIMARY,
|
||||
// DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(false);
|
||||
@@ -123,10 +126,10 @@ public class Drive extends SubsystemBase {
|
||||
m_leftBackMotor.setSensorPhase(true);*/
|
||||
|
||||
/* 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_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_rightFrontMotor.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* deadbands */
|
||||
@@ -170,10 +173,10 @@ public class Drive extends SubsystemBase {
|
||||
* Max out the peak output (for all modes).
|
||||
* However you can limit the output of a given PID object with configClosedLoopPeakOutput().
|
||||
*/
|
||||
m_leftFrontMotor.configPeakOutputForward(+0.5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configPeakOutputReverse(-0.5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputForward(+0.5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputReverse(-0.5, 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.
|
||||
@@ -184,7 +187,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
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_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* configAuxPIDPolarity(boolean invert, int timeoutMs)
|
||||
@@ -296,9 +299,12 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
public void runVelocityPID(double targetVel) {
|
||||
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, 0);
|
||||
//m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
//m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVelLeft, DemandType.AuxPID, 0);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0);
|
||||
//m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0);
|
||||
}
|
||||
|
||||
public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){
|
||||
|
||||
Reference in New Issue
Block a user