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

Merge pull request #25 from Team4388/fix-drive-straight-position-pid

Browse Source 
Development on Pos, Vel, and Rot PIDs while also adding MM PID.
This commit is contained in:
Keenan D. Buckley
2020-02-11 04:39:51 +00:00
committed by GitHub
parent fdc8e37b5b 767c61016f
commit c94de7fdc6
6 changed files with 204 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -32,8 +32,8 @@ public final class Constants {
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;
public static final int DRIVE_CRUISE_VELOCITY = 20000;
public static final int DRIVE_ACCELERATION = 7000;
/* Remote Sensors */
public final static int REMOTE_0 = 0;
src/main/java/frc4388/robot/RobotContainer.java
+13 -5
View File
@@ -16,7 +16,10 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.DriveWithJoystickAuxPID;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
@@ -88,11 +91,8 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
// test command to spin the robot while pressing A on the driver controller
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive);
//new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
// .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
/* Operator Buttons */
// activates "Lit Mode"
@@ -105,15 +105,23 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
/* PID Test Command */
// runs velocity PID while driving straight
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
//new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController()));
// resets the yaw of the pigeon
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
// turn 45 degrees
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+70
View File
@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* 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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionMM extends CommandBase {
Drive m_drive;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
boolean isGoneFast;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
System.err.println("PID START \n | \n |");
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
isGoneFast = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
m_drive.runMotionMagicPID(m_targetPosOut, 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() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
return true;
} else {
if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
isGoneFast = true;
}
return false;
}
}
}
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+15 -6
View File
@@ -14,8 +14,10 @@ import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
double m_targetPos;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
int i;
/**
* Creates a new DriveToDistancePID.
@@ -25,22 +27,27 @@ public class DriveStraightToPositionPID extends CommandBase {
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
SmartDashboard.putNumber("Distance Target Inches", targetPos);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
//System.err.println("PID START \n | \n |");
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
i = 0;
}
// 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.runDriveStraightPositionPID(m_targetPos, m_targetGyro);
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro);
}
// Called once the command ends or is interrupted.
@@ -51,9 +58,11 @@ public class DriveStraightToPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
return true;
} else {
i++;
//System.err.println(i);
return false;
}
}
src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java
+79
View File
@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* 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 com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickAuxPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
long lastTime;
IHandController m_controller;
/**
* Creates a new DriveWithJoystickAuxPID.
*/
public DriveWithJoystickAuxPID(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
lastTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
long deltaTime = System.currentTimeMillis() - lastTime;
lastTime = System.currentTimeMillis();
if (moveInput >= 0){
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
m_targetGyro += 2 * steerInput * deltaTime;
m_targetGyro = MathUtil.clamp(m_targetGyro,
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
System.err.println("Target: " + m_targetGyro);
System.err.println("Current: " + currentGyro);
System.err.println();
}
// 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/subsystems/Drive.java
+23 -7
View File
@@ -7,6 +7,7 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
@@ -104,6 +105,15 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionSCurveStrength(0, 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);
@@ -134,8 +144,8 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
/* Diff Signal */
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference,
@@ -325,8 +335,17 @@ public class Drive extends SubsystemBase {
m_driveTrain.arcadeDrive(move, steer);
}
public void driveWithInputAux(double move, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
/**
* Runs a position PID while driving straight (has not been tested)
* Runs a position PID while driving straight
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
@@ -334,7 +353,6 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetPos *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
@@ -349,8 +367,6 @@ public class Drive extends SubsystemBase {
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
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.AuxOutput1);
@@ -366,7 +382,7 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();