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

Got DriveStraightAtVelocityPID Working

Browse Source
This commit is contained in:
aarav18
2020-02-06 18:36:04 -07:00
parent 4dc07d718a
commit 987a3a2647
6 changed files with 155 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -3
View File
@@ -29,8 +29,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.0, 0.0, 0.0, 0.0448767878, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.0);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 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.3);
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 = 2000;
public static final int DRIVE_ACCELERATION = 1000;
@@ -50,7 +50,7 @@ public final class Constants {
public final static int SLOT_MOTION_MAGIC = 3;
/* Drive Train Characteristics */
public static final double TICKS_PER_MOTOR_REV = 2048;
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 WHEEL_DIAMETER_INCHES = 6;
public static final double TICKS_PER_GYRO_REV = 8192;
src/main/java/frc4388/robot/Robot.java
-2
View File
@@ -98,7 +98,6 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
@@ -113,7 +112,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+3 -2
View File
@@ -17,7 +17,8 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DriveAtVelocityPID;
import frc4388.robot.commands.DriveToDistanceMM;
import frc4388.robot.commands.DriveToDistancePID;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.subsystems.Drive;
@@ -79,7 +80,7 @@ public class RobotContainer {
/* PID Test Command */
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 150))
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000))
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+51
View File
@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveStraightAtVelocityPID extends CommandBase {
Drive m_drive;
double m_targetVel;
double m_targetGyro;
/**
* Creates a new DriveVelocityControlPID.
*/
public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetVel = targetVel;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runVelocityPID(m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DriveToDistancePID.java → src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+2 -2
View File
@@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveToDistancePID extends CommandBase {
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
double m_distance;
double m_leftTarget;
@@ -23,7 +23,7 @@ public class DriveToDistancePID extends CommandBase {
* @param subsystem drive subsystem
* @param distance distance to travel in inches
*/
public DriveToDistancePID(Drive subsystem, double distance) {
public DriveStraightToPositionPID(Drive subsystem, double distance) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance * DriveConstants.TICKS_PER_INCH;
src/main/java/frc4388/robot/subsystems/Drive.java
+96 -105
View File
@@ -88,109 +88,117 @@ public class Drive extends SubsystemBase {
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);
//Beginning of nada
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.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);
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);
/* 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
/* Diff Signal */
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, 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);
/* Scale Feedback by 0.5 to half the sum of Distance */
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_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,
/* 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);
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);
m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
//setDriveTrainGains("Distance PID", m_gainsDistance);
//m_chooser.addOption("Velocity PID", m_gainsVelocity);
m_chooser.addOption("Velocity PID", m_gainsVelocity);
//setDriveTrainGains("Velocity PID", m_gainsVelocity);
//m_chooser.addOption("Turning PID", m_gainsTurning);
m_chooser.addOption("Turning PID", m_gainsTurning);
//setDriveTrainGains("Turning PID", m_gainsTurning);
//m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
//setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
//Shuffleboard.getTab("PID").add(m_chooser);
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.
@@ -199,56 +207,42 @@ 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();
@Override
public void periodic() {
try {
//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());
//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());
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
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());
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 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("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 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));
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
/*Gains gains = m_chooser.getSelected();
if (gains.equals(gainsOld)) {
SmartDashboard.putNumber("P Value Drive", gains.kP);
SmartDashboard.putNumber("I Value Drive", gains.kI);
SmartDashboard.putNumber("D Value Drive", gains.kD);
SmartDashboard.putNumber("F Value Drive", gains.kF);
}
gains.kP = SmartDashboard.getNumber("P Value Drive", gains.kP);
gains.kI = SmartDashboard.getNumber("I Value Drive", gains.kI);
gains.kD = SmartDashboard.getNumber("D Value Drive", gains.kD);
gains.kF = SmartDashboard.getNumber("F Value Drive", gains.kF);
setDriveTrainGains(m_chooser.getName(), gains);
gainsOld = gains;*/
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));
SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
} catch (Exception e) {
System.err.println("Error in the Drive Subsystem");
@@ -328,15 +322,12 @@ public class Drive extends SubsystemBase {
long last = 0;
public void runVelocityPID(double targetVel, double targetGyro) {
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightBackMotor.follow(m_rightFrontMotor);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, 1000);//, DemandType.AuxPID, targetGyro);
long now = System.nanoTime();
SmartDashboard.putNumber("Loop Time", (now-last)/1000000);
last = now;
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput);
//m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetVel *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
//m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}