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 branch 'add-ramsete-controller-and-trajectories' of https://github.com/Team4388/RiseOfRidgebotics2020 into add-ramsete-controller-and-trajectories

Browse Source
This commit is contained in:
Aarav Shah
2020-02-18 17:37:15 -07:00
parent 2c47b71534 45db7a32d7
commit ac777e611e
3 changed files with 44 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -6
View File
@@ -29,12 +29,12 @@ 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.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;
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.0, 0.05, 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;
/* Trajectory Constants */
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+22 -9
View File
@@ -16,9 +16,10 @@ import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 500;
long m_deadTimeout = 100;
IHandController m_controller;
/**
@@ -56,13 +57,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
/* If move stick is being used */
if (moveInput != 0) {
m_deadTimeMove = m_currTime;
m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
+ (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* If move stick has not been pressed for 1 sec */
/* If move stick has been pressed within 1 sec */
if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
@@ -74,7 +77,6 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
m_deadTimeSteer = m_currTime;
}
/* If steer stick has not been used for 1 sec */
else {
@@ -103,21 +105,30 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move, m_targetGyro);
System.out.println("Driving with Input Aux with Target: " + m_targetGyro);
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
updateGyroTarget(steer);
m_drive.runDriveVelocityPID(0, m_targetGyro);
System.out.println("Driving with Velocity PID with Target: " + m_targetGyro);
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
System.out.println("Turning with Target: " + m_targetGyro);
/* if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity() > 3000) {
m_drive.driveWithInputAux(0, m_targetGyro);
System.out.println("!Turning with Target: " + m_targetGyro);
} else {
m_drive.runDriveVelocityPID(0, m_targetGyro);
System.out.println("Turning with Target: " + m_targetGyro);
}*/
}
/**
* If AuxPID is enabled, then update using the steer input
*/
private void updateGyroTarget(double steerInput) {
m_targetGyro += 2 * steerInput * m_deltaTime;
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
@@ -128,7 +139,9 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
m_targetGyro = m_currentGyro;
//m_targetGyro = m_currentGyro;
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1)
+ (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1));
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Drive.java
+16 -16
View File
@@ -59,7 +59,7 @@ public class Drive extends SubsystemBase {
public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
//public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
public final DifferentialDriveOdometry m_odometry;
@@ -105,12 +105,12 @@ public class Drive extends SubsystemBase {
//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);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
//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);
//m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
//m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
//m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -126,15 +126,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.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, 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);
//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.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, 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);
/* PID for Back Motor control in Auto */
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);