--- /dev/null
+package org.usfirst.frc.team3501.robot;
+
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * Copied/modified from Team 254's 2014 codebase. Thanks so much guys!
+ *
+ * CheesyDriveHelper implements the calculations used in CheesyDrive, sending
+ * power to the motors.
+ */
+public class CheesyDriveHelper {
+
+ private DriveTrain drive;
+ double oldWheel, quickStopAccumulator;
+ private double throttleDeadband = 0.02;
+ private double wheelDeadband = 0.02;
+
+ public CheesyDriveHelper(DriveTrain drive) {
+ this.drive = drive;
+ }
+
+ public void cheesyDrive(double throttle, double wheel, boolean isHighGear) {
+ if (DriverStation.getInstance().isAutonomous()) {
+ return;
+ }
+
+ double wheelNonLinearity;
+
+ wheel = handleDeadband(wheel, wheelDeadband);
+ throttle = handleDeadband(throttle, throttleDeadband);
+
+ double negInertia = wheel - oldWheel;
+ oldWheel = wheel;
+
+ if (isHighGear) {
+ wheelNonLinearity = 0.6;
+ // Apply a sin function that's scaled to make it feel better.
+ wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+ / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+ wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+ / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+ } else {
+ wheelNonLinearity = 0.5;
+ // Apply a sin function that's scaled to make it feel better.
+ wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+ / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+ wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+ / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+ wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+ / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+ }
+
+ double leftPwm, rightPwm, overPower;
+ double sensitivity;
+
+ double angularPower;
+ double linearPower;
+
+ // Negative inertia!
+ double negInertiaAccumulator = 0.0;
+ double negInertiaScalar;
+ if (isHighGear) {
+ negInertiaScalar = 5.0;
+ sensitivity = .75;
+ } else {
+ if (wheel * negInertia > 0) {
+ negInertiaScalar = 2.5;
+ } else {
+ if (Math.abs(wheel) > 0.65) {
+ negInertiaScalar = 5.0;
+ } else {
+ negInertiaScalar = 3.0;
+ }
+ }
+ sensitivity = .75;
+ }
+ double negInertiaPower = negInertia * negInertiaScalar;
+ negInertiaAccumulator += negInertiaPower;
+
+ wheel = wheel + negInertiaAccumulator;
+ if (negInertiaAccumulator > 1) {
+ negInertiaAccumulator -= 1;
+ } else if (negInertiaAccumulator < -1) {
+ negInertiaAccumulator += 1;
+ } else {
+ negInertiaAccumulator = 0;
+ }
+ linearPower = throttle;
+
+ overPower = 0.0;
+ angularPower = Math.abs(throttle) * wheel * sensitivity
+ - quickStopAccumulator;
+ if (quickStopAccumulator > 1) {
+ quickStopAccumulator -= 1;
+ } else if (quickStopAccumulator < -1) {
+ quickStopAccumulator += 1;
+ } else {
+ quickStopAccumulator = 0.0;
+ }
+
+ rightPwm = leftPwm = linearPower;
+ leftPwm += angularPower;
+ rightPwm -= angularPower;
+
+ if (leftPwm > 1.0) {
+ rightPwm -= overPower * (leftPwm - 1.0);
+ leftPwm = 1.0;
+ } else if (rightPwm > 1.0) {
+ leftPwm -= overPower * (rightPwm - 1.0);
+ rightPwm = 1.0;
+ } else if (leftPwm < -1.0) {
+ rightPwm += overPower * (-1.0 - leftPwm);
+ leftPwm = -1.0;
+ } else if (rightPwm < -1.0) {
+ leftPwm += overPower * (-1.0 - rightPwm);
+ rightPwm = -1.0;
+ }
+ drive.setMotorSpeeds(leftPwm, rightPwm);
+
+ }
+
+ public double handleDeadband(double val, double deadband) {
+ return (Math.abs(val) > Math.abs(deadband)) ? val : 0.0;
+ }
+}
\ No newline at end of file
package org.usfirst.frc.team3501.robot.subsystems;
+import org.usfirst.frc.team3501.robot.CheesyDriveHelper;
import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
private DoubleSolenoid leftGearPiston, rightGearPiston;
+ private CheesyDriveHelper cheese;
+
// Drivetrain specific constants that relate to the inches per pulse value for
// the encoders
Constants.DriveTrain.RIGHT_SHIFT_MODULE,
Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
+
+ cheese = new CheesyDriveHelper(this);
}
@Override
public void joystickDrive(double left, double right) {
// Handle flipping of the "front" of the robot
double k = (isFlipped() ? -1 : 1);
-
- robotDrive.tankDrive(-left * k, -right * k);
+ cheese.cheesyDrive(-left * k, -right,
+ getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
}
public void setMotorSpeeds(double left, double right) {
double k = (isFlipped() ? -1 : 1);
- robotDrive.tankDrive(-left * k, -right * k);
+ cheese.cheesyDrive(-left * k, -right,
+ getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
}
/**