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

Tweaks to Deadassist and Work on Motion Magic

Browse Source
This commit is contained in:
Aarav Shah
2020-02-21 20:46:52 -07:00
parent 9f1aff6c8d
commit 5f989fb21a
6 changed files with 27 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -6
View File
@@ -29,12 +29,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.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.45);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.45);
//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.5);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
/* Trajectory Constants */
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -145,7 +145,7 @@ public class RobotContainer {
// resets the yaw of the pigeon
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whileHeld(new RunCommand(() -> m_robotDrive.runMotionMagicPID(12, 0), m_robotDrive));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 36));
// turn 45 degrees
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+6 -1
View File
@@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase {
double m_targetPosOut;
double m_targetGyro;
boolean isGoneFast;
int i;
/**
* Creates a new DriveToDistancePID.
@@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
isGoneFast = false;
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase {
//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);
SmartDashboard.putBoolean("MM Run", true);
i++;
}
// Called once the command ends or is interrupted.
@@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase {
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
SmartDashboard.putBoolean("MM Run", false);
return true;
} else {
if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
isGoneFast = true;
}
return false;
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+1 -1
View File
@@ -48,7 +48,7 @@ public class DriveWithJoystick extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
double cosMultiplier = .45;
double cosMultiplier = .55;
double deadzone = .2;
if (steerInput > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+4 -4
View File
@@ -99,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runDriveWithInput(double move, double steerInput) {
double cosMultiplier = .45;
double cosMultiplier = .55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
@@ -119,7 +119,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runStoppedTurn(double steer) {
double cosMultiplier = 0.70;
double cosMultiplier = 0.55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
@@ -145,8 +145,8 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
private void updateGyroTarget(double steerInput) {
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2));
}
/**
src/main/java/frc4388/robot/subsystems/Drive.java
+11 -10
View File
@@ -74,7 +74,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;
@@ -149,15 +149,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);
@@ -456,6 +456,7 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);