-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
-Added CDH + implementation -Let Spaghetti break gravity constant -Fixed launcher
- Loading branch information
1 parent
33ede69
commit 7f1443a
Showing
8 changed files
with
185 additions
and
17 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,147 @@ | ||
| package com.team3925.robot2016; | ||
|
|
||
| import com.team3925.robot2016.subsystems.DriveTrain; | ||
| import com.team3925.robot2016.util.DriveTrainSignal; | ||
| import com.team3925.robot2016.util.MiscUtil; | ||
|
|
||
| import edu.wpi.first.wpilibj.DriverStation; | ||
|
|
||
| /** | ||
| * 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; | ||
| private DriveTrainSignal signal = new DriveTrainSignal(0, 0); | ||
|
|
||
| public CheesyDriveHelper(DriveTrain drive) { | ||
| this.drive = drive; | ||
| } | ||
|
|
||
| public void cheesyDrive(double throttle, double wheel, boolean isQuickTurn, | ||
| 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 = 4.0; | ||
| sensitivity = Constants.kDriveSensitivity; | ||
| } else { | ||
| if (wheel * negInertia > 0) { | ||
| negInertiaScalar = 2.5; | ||
| } else { | ||
| if (Math.abs(wheel) > 0.65) { | ||
| negInertiaScalar = 5.0; | ||
| } else { | ||
| negInertiaScalar = 3.0; | ||
| } | ||
| } | ||
| sensitivity = .85; // Constants.sensitivityLow.getDouble(); | ||
| } | ||
| 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; | ||
|
|
||
| // Quickturn! | ||
| if (isQuickTurn) { | ||
| if (Math.abs(linearPower) < 0.2) { | ||
| double alpha = 0.1; | ||
| quickStopAccumulator = (1 - alpha) * quickStopAccumulator | ||
| + alpha * MiscUtil.limit(wheel, 1.0) * 5; | ||
| } | ||
| overPower = 1.0; | ||
| if (isHighGear) { | ||
| sensitivity = 1.0; | ||
| } else { | ||
| sensitivity = 1.0; | ||
| } | ||
| angularPower = wheel; | ||
| } else { | ||
| 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; | ||
| } | ||
| signal.left = leftPwm; | ||
| signal.right = rightPwm; | ||
| drive.setMotorSpeeds(signal); | ||
| } | ||
|
|
||
| public double handleDeadband(double val, double deadband) { | ||
| return (Math.abs(val) > Math.abs(deadband)) ? val : 0.0; | ||
| } | ||
|
|
||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters