Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Issue 56 - Improved Cheesy drive logic #97

Merged
merged 1 commit into from
Jan 15, 2017
Merged

Issue 56 - Improved Cheesy drive logic #97

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
136 changes: 131 additions & 5 deletions strongback/src/org/strongback/drive/TankDrive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,10 @@
*/
public class TankDrive implements Requirable {

public static enum CheesyAlgorithm {
SIMPLE, INERTIA
}

public static final double DEFAULT_MINIMUM_SPEED = 0.02;
public static final double DEFAULT_MAXIMUM_SPEED = 1.0;
public static final DoubleToDoubleFunction DEFAULT_SPEED_LIMITER = Values.symmetricLimiter(DEFAULT_MINIMUM_SPEED,
Expand All @@ -43,6 +47,7 @@ public class TankDrive implements Requirable {
private static final double SENSITIVITY_HIGH = 0.75;
private static final double SENSITIVITY_LOW = 0.75;
private static final double HALF_PI = Math.PI / 2.0;
private static final double SENSITIVITY_TURN = 1.0;

private final Motor left;
private final Motor right;
Expand All @@ -54,7 +59,7 @@ public class TankDrive implements Requirable {

/**
* Creates a new DriveSystem subsystem that uses the supplied drive train and no shifter. The voltage send to the drive
* train is limited to [-1.0,1.0].
* train is limited to [-1.0,1.0] with the exception of the (-0.02,0.02) deadband around the zero point.
*
* @param left the left motor on the drive train for the robot; may not be null
* @param right the right motor on the drive train for the robot; may not be null
Expand All @@ -65,7 +70,7 @@ public TankDrive(Motor left, Motor right) {

/**
* Creates a new DriveSystem subsystem that uses the supplied drive train and optional shifter. The voltage send to the
* drive train is limited to [-1.0,1.0].
* drive train is limited to [-1.0,1.0] with the exception of the (-0.02,0.02) deadband around the zero point.
*
* @param left the left motor on the drive train for the robot; may not be null
* @param right the right motor on the drive train for the robot; may not be null
Expand All @@ -83,7 +88,7 @@ public TankDrive(Motor left, Motor right, Relay shifter) {
* @param right the right motor on the drive train for the robot; may not be null
* @param shifter the optional shifter used to put the transmission into high gear; may be null
* @param speedLimiter the function that limits the speed sent to the drive train; if null, then a default clamping function
* is used to limit to the range [-1.0,1.0]
* is used to limit to the range [-1.0,1.0] with the exception of the (-0.02,0.02) deadband around the zero point
*/
public TankDrive(Motor left, Motor right, Relay shifter, DoubleToDoubleFunction speedLimiter) {
this.left = left;
Expand Down Expand Up @@ -231,6 +236,66 @@ private static double dampen(double wheel, double wheelNonLinearity) {
return Math.sin(factor * wheel) / Math.sin(factor);
}

/**
* Provide "cheesy drive" steering using a steering wheel and throttle. This function lets you directly provide joystick
* values from any source.
* <p>
* "Cheesy Drive" simply means that the "turning" stick controls the curvature of the robot's path rather than its rate of
* heading change. This helps make the robot more controllable at high speeds. Also handles the robot's quick turn
* functionality - "quick turn" overrides constant-curvature turning for turn-in-place maneuvers.
* <p>
* This is equivalent to calling {@code cheesy(throttle, wheel, isQuickTurn, CheesyAlgorithm.SIMPLE)}.
*
* @param throttle the value of the throttle, which is generally in the range -1 to 1 inclusive but limited by the speed
* limiter provided in the constructor
* @param wheel the value of the steering wheel, which is generally in the range -1 to 1 inclusive but limited by the speed
* limiter provided in the constructor and where negative values turn right and positive values turn left
* @param isQuickTurn true if the quick-turn button is pressed
* @see <a href="https://github.com/Team254/FRC-2016-Public/blob/master/src/com/team254/frc2016/CheesyDriveHelper.java">Team
* 254 Cheesy Drive logic from 2016</a>
*/
public void cheesy(double throttle, double wheel, boolean isQuickTurn) {
cheesy2016(throttle, wheel, isQuickTurn);
}

/**
* Provide "cheesy drive" steering using a steering wheel and throttle. This function lets you directly provide joystick
* values from any source.
* <p>
* "Cheesy Drive" simply means that the "turning" stick controls the curvature of the robot's path rather than its rate of
* heading change. This helps make the robot more controllable at high speeds. Also handles the robot's quick turn
* functionality - "quick turn" overrides constant-curvature turning for turn-in-place maneuvers.
* <p>
* This method supports two different algorithms for cheesy drive:
* <ul>
* <li><i>simple<i> - an improved and simplified algorithm from
* <a href="https://github.com/Team254/FRC-2016-Public/blob/master/src/com/team254/frc2016/CheesyDriveHelper.java">Team 254
* Cheesy Drive logic from 2016</a>; and</li>
* <li><i>inertia<i> - an earlier algorithm that uses inertia terms to non-linearly smooth the outputs, from
* <a href="https://github.com/Team254/FRC-2016-Public/blob/master/src/com/team254/frc2016/CheesyDriveHelper.java">Team 254
* Cheesy Drive logic from 2014</a></li>
* </ul>
*
* @param throttle the value of the throttle, which is generally in the range -1 to 1 inclusive but limited by the speed
* limiter provided in the constructor
* @param wheel the value of the steering wheel, which is generally in the range -1 to 1 inclusive but limited by the speed
* limiter provided in the constructor and where negative values turn right and positive values turn left
* @param isQuickTurn true if the quick-turn button is pressed
* @param algorithm the algorithm to use; may not be null
* @see <a href="https://github.com/Team254/FRC-2016-Public/blob/master/src/com/team254/frc2016/CheesyDriveHelper.java">Team
* 254 Cheesy Drive logic from 2016</a>
*/
public void cheesy(double throttle, double wheel, boolean isQuickTurn, CheesyAlgorithm algorithm) {
switch (algorithm) {
case INERTIA:
cheesy2014(throttle, wheel, isQuickTurn);
break;
case SIMPLE:
cheesy2016(throttle, wheel, isQuickTurn);
break;
}
}

/**
* Provide "cheesy drive" steering using a steering wheel and throttle. This function lets you directly provide joystick
* values from any source.
Expand All @@ -240,9 +305,9 @@ private static double dampen(double wheel, double wheelNonLinearity) {
* left.
* @param isQuickTurn true if the quick-turn button is pressed
* @see <a href="https://github.com/Team254/FRC-2014/blob/master/src/com/team254/frc2014/CheesyDriveHelper.java">Team 254
* Cheesy Drive logic</a>
* Cheesy Drive logic from 2014</a>
*/
public void cheesy(double throttle, double wheel, boolean isQuickTurn) {
private void cheesy2014(double throttle, double wheel, boolean isQuickTurn) {

wheel = speedLimiter.applyAsDouble(wheel);
throttle = speedLimiter.applyAsDouble(throttle);
Expand Down Expand Up @@ -348,4 +413,65 @@ public void cheesy(double throttle, double wheel, boolean isQuickTurn) {
right.setSpeed(rightPwm);
}

/**
* Provide a simpler "cheesy drive" steering using a steering wheel and throttle. This function lets you directly provide
* joystick values from any source.
* <p>
* "Cheesy Drive" simply means that the "turning" stick controls the curvature of the robot's path rather than its rate of
* heading change. This helps make the robot more controllable at high speeds. Also handles the robot's quick turn
* functionality - "quick turn" overrides constant-curvature turning for turn-in-place maneuvers.
*
* @param throttle the value of the throttle, which is generally in the range -1 to 1 inclusive but limited by the speed
* limiter provided in the constructor
* @param wheel the value of the steering wheel, which is generally in the range -1 to 1 inclusive but limited by the speed
* limiter provided in the constructor and where negative values turn right and positive values turn left
* @param isQuickTurn true if the quick-turn button is pressed
* @see <a href="https://github.com/Team254/FRC-2016-Public/blob/master/src/com/team254/frc2016/CheesyDriveHelper.java">Team
* 254 Cheesy Drive logic from 2016</a>
*/
private void cheesy2016(double throttle, double wheel, boolean isQuickTurn) {
wheel = speedLimiter.applyAsDouble(wheel);
throttle = speedLimiter.applyAsDouble(throttle);

double overPower;
double angularPower;

if (isQuickTurn) {
if (Math.abs(throttle) < DEFAULT_MINIMUM_SPEED) {
double alpha = 0.1;
quickStopAccumulator = (1 - alpha) * quickStopAccumulator + alpha * Values.symmetricLimit(0.0, wheel, 1.0) * 2;
}
overPower = 1.0;
angularPower = wheel;
} else {
overPower = 0.0;
angularPower = Math.abs(throttle) * wheel * SENSITIVITY_TURN - quickStopAccumulator;
if (quickStopAccumulator > 1) {
quickStopAccumulator -= 1;
} else if (quickStopAccumulator < -1) {
quickStopAccumulator += 1;
} else {
quickStopAccumulator = 0.0;
}
}

double rightPwm = throttle - angularPower;
double leftPwm = throttle + 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;
}
left.setSpeed(leftPwm);
right.setSpeed(rightPwm);
}

}