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

Added PID Stuff for pigeon go straight

Browse Source
This commit is contained in:
aarav18
2020-01-23 16:42:20 -07:00
parent 2d1ba14cfd
commit 7008ceb453
4 changed files with 75 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -28,10 +28,10 @@ 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.2, 0, 1.0);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);
public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
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 = 15000;
public static final int DRIVE_ACCELERATION = 6000;
src/main/java/frc4388/robot/RobotContainer.java
+3 -3
View File
@@ -79,13 +79,13 @@ public class RobotContainer {
/* PID Test Command */
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveToDistancePID(m_robotDrive, 36));
.whileHeld(new RunCommand(() -> m_robotDrive.runVelocityPID(2000), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new DriveToDistanceMM(m_robotDrive, 36));
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 12));
.whileHeld(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(() -> System.err.print("interrupt"), m_robotDrive));
src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java
+8 -5
View File
@@ -7,6 +7,8 @@
package frc4388.robot.commands;
import com.ctre.phoenix.motorcontrol.FollowerType;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
@@ -24,22 +26,23 @@ 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;*/;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_targetVel;
m_rightTarget = -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_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget);
//m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget);
//m_drive.runVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget);
//m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Drive.java
+60 -21
View File
@@ -7,15 +7,19 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -78,6 +82,12 @@ public class Drive extends SubsystemBase {
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);
/* 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);
@@ -88,33 +98,40 @@ public class Drive extends SubsystemBase {
DriveConstants.DRIVE_TIMEOUT_MS);
/* Scale Feedback by 0.5 to half the sum of Distance */
m_rightFrontMotor.configSelectedFeedbackCoefficient( 0.5, // Coefficient
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient
DriveConstants.PID_PRIMARY, // PID Slot of Source
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/* Scale the Feedback Sensor using a coefficient */
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0,
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1,
DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
m_rightFrontMotor.setSensorPhase(false);
m_leftFrontMotor.setSensorPhase(false);
/* Set status frame periods to ensure we don't have stale data */
/* 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(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
m_rightFrontMotor.setSensorPhase(true);
m_leftFrontMotor.setSensorPhase(false);
/*m_rightBackMotor.setSensorPhase(true);
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_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 */
m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE
m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed
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.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -149,6 +166,15 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("D Value Drive", gains.kD);
SmartDashboard.putNumber("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(+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);
/**
* 1ms per loop. PID loop can be slowed down if need be.
* For example,
@@ -157,15 +183,15 @@ public class Drive extends SubsystemBase {
* - sensor movement is very slow causing the derivative error to be near zero.
*/
int closedLoopTimeMs = 1;
m_leftFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
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(true, DriveConstants.DRIVE_TIMEOUT_MS);
}
Gains gainsOld;
@@ -264,17 +290,30 @@ public class Drive extends SubsystemBase {
}
public void runPositionPID(WPI_TalonFX talon, double targetPos) {
talon.selectProfileSlot(DriveConstants.SLOT_DISTANCE , DriveConstants.PID_PRIMARY);
talon.set(TalonFXControlMode.Position, targetPos);
}
public void runVelocityPID(WPI_TalonFX talon, double targetVel) {
talon.set(TalonFXControlMode.Velocity, targetVel);
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.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
}
public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){
talon.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
talon.set(TalonFXControlMode.MotionMagic, targetPos);
}
public void runTurningPID(double targetAngle){
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, 0, DemandType.AuxPID, targetAngle);
//m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_rightFrontMotor.set(DemandType.AuxPID, 0);
}
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);