73b73966f6bfa1d5186286fe7b854b88187af115
1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
5 import edu
.wpi
.first
.wpilibj
.DriverStation
;
8 * Copied/modified from Team 254's 2014 codebase. Thanks so much guys!
10 * CheesyDriveHelper implements the calculations used in CheesyDrive, sending
11 * power to the motors.
13 public class CheesyDriveHelper
{
15 private DriveTrain drive
;
16 double oldWheel
, quickStopAccumulator
;
17 private double throttleDeadband
= 0.02;
18 private double wheelDeadband
= 0.02;
20 public CheesyDriveHelper(DriveTrain drive
) {
24 public void cheesyDrive(double throttle
, double wheel
, boolean isHighGear
) {
25 if (DriverStation
.getInstance().isAutonomous()) {
29 double wheelNonLinearity
;
31 wheel
= handleDeadband(wheel
, wheelDeadband
);
32 throttle
= handleDeadband(throttle
, throttleDeadband
);
34 double negInertia
= wheel
- oldWheel
;
38 wheelNonLinearity
= 0.6;
39 // Apply a sin function that's scaled to make it feel better.
40 wheel
= Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
* wheel
)
41 / Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
);
42 wheel
= Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
* wheel
)
43 / Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
);
45 wheelNonLinearity
= 0.5;
46 // Apply a sin function that's scaled to make it feel better.
47 wheel
= Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
* wheel
)
48 / Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
);
49 wheel
= Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
* wheel
)
50 / Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
);
51 wheel
= Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
* wheel
)
52 / Math
.sin(Math
.PI
/ 2.0 * wheelNonLinearity
);
55 double leftPwm
, rightPwm
, overPower
;
62 double negInertiaAccumulator
= 0.0;
63 double negInertiaScalar
;
65 negInertiaScalar
= 5.0;
68 if (wheel
* negInertia
> 0) {
69 negInertiaScalar
= 2.5;
71 if (Math
.abs(wheel
) > 0.65) {
72 negInertiaScalar
= 5.0;
74 negInertiaScalar
= 3.0;
79 double negInertiaPower
= negInertia
* negInertiaScalar
;
80 negInertiaAccumulator
+= negInertiaPower
;
82 wheel
= wheel
+ negInertiaAccumulator
;
83 if (negInertiaAccumulator
> 1) {
84 negInertiaAccumulator
-= 1;
85 } else if (negInertiaAccumulator
< -1) {
86 negInertiaAccumulator
+= 1;
88 negInertiaAccumulator
= 0;
90 linearPower
= throttle
;
93 angularPower
= Math
.abs(throttle
) * wheel
* sensitivity
94 - quickStopAccumulator
;
95 if (quickStopAccumulator
> 1) {
96 quickStopAccumulator
-= 1;
97 } else if (quickStopAccumulator
< -1) {
98 quickStopAccumulator
+= 1;
100 quickStopAccumulator
= 0.0;
103 rightPwm
= leftPwm
= linearPower
;
104 leftPwm
+= angularPower
;
105 rightPwm
-= angularPower
;
108 rightPwm
-= overPower
* (leftPwm
- 1.0);
110 } else if (rightPwm
> 1.0) {
111 leftPwm
-= overPower
* (rightPwm
- 1.0);
113 } else if (leftPwm
< -1.0) {
114 rightPwm
+= overPower
* (-1.0 - leftPwm
);
116 } else if (rightPwm
< -1.0) {
117 leftPwm
+= overPower
* (-1.0 - rightPwm
);
120 drive
.setMotorSpeeds(leftPwm
, rightPwm
);
124 public double handleDeadband(double val
, double deadband
) {
125 return (Math
.abs(val
) > Math
.abs(deadband
)) ? val
: 0.0;