|
1
|
|
|
package org.usfirst.frc.team3695.robot.subsystems;
|
|
2
|
|
|
|
|
3
|
|
|
import org.usfirst.frc.team3695.robot.Constants;
|
|
4
|
|
|
import org.usfirst.frc.team3695.robot.commands.ManualCommandDrive;
|
|
5
|
|
|
import org.usfirst.frc.team3695.robot.enumeration.Drift;
|
|
6
|
|
|
import org.usfirst.frc.team3695.robot.util.Xbox;
|
|
7
|
|
|
|
|
8
|
|
|
import com.ctre.CANTalon;
|
|
9
|
|
|
|
|
10
|
|
|
import edu.wpi.first.wpilibj.Joystick;
|
|
11
|
|
|
import edu.wpi.first.wpilibj.command.Subsystem;
|
|
12
|
|
|
|
|
13
|
|
|
/** VROOM VROOM */
|
|
14
|
|
|
public class SubsystemDrive extends Subsystem {
|
|
15
|
|
|
|
|
16
|
|
|
/// drift variables
|
|
17
|
|
|
public static int OFFSET, // the degrees the bot is tilted inward
|
|
|
|
|
|
|
18
|
|
|
METADIRECTION; // on button down, this resets to 0; the angle relative to that initial angle (changes on physical rotation)
|
|
|
|
|
|
|
19
|
|
|
// pretty much the cumulative angle since button down, minus offset
|
|
20
|
|
|
public static double ROT_SPEED, // the speed of rotation (min 0, max 10.00)
|
|
|
|
|
|
|
21
|
|
|
ROT_RADIUS; // turn radius in inches
|
|
|
|
|
|
|
22
|
|
|
public static boolean IN_DRIFT, // if the bot is currently in a drift
|
|
|
|
|
|
|
23
|
|
|
DRIFT_IS_CW; // if the drift is turning right
|
|
|
|
|
|
|
24
|
|
|
|
|
25
|
|
|
// instantiate the CANTalons here
|
|
26
|
|
|
// EX: private CANTalon left1;
|
|
27
|
|
|
|
|
28
|
|
|
|
|
29
|
|
|
/** runs at robot boot */
|
|
30
|
|
|
public void initDefaultCommand() {
|
|
31
|
|
|
setDefaultCommand(new ManualCommandDrive()); }
|
|
32
|
|
|
|
|
33
|
|
|
|
|
34
|
|
|
/** converts RPM to inches per second */
|
|
35
|
|
|
public static final double rpm2ips(double rpm) {
|
|
36
|
|
|
return rpm / 60.0 * Constants.WHEEL_DIAMETER * Math.PI; }
|
|
|
|
|
|
|
37
|
|
|
|
|
38
|
|
|
|
|
39
|
|
|
/** converts an inches per second number to RPM */
|
|
40
|
|
|
public static final double ips2rpm(double ips) {
|
|
41
|
|
|
return ips * 60.0 / Constants.WHEEL_DIAMETER / Math.PI; }
|
|
|
|
|
|
|
42
|
|
|
|
|
43
|
|
|
|
|
44
|
|
|
/** converts rotations to distance traveled in inches */
|
|
45
|
|
|
public static final double rot2in(double rot) {
|
|
46
|
|
|
return rot * Constants.WHEEL_DIAMETER * Math.PI; }
|
|
47
|
|
|
|
|
48
|
|
|
|
|
49
|
|
|
/** converts distance traveled in inches to rotations */
|
|
50
|
|
|
public static final double in2rot(double in) {
|
|
51
|
|
|
return in / Constants.WHEEL_DIAMETER / Math.PI; }
|
|
52
|
|
|
|
|
53
|
|
|
/** gives birth to the CANTalons */
|
|
54
|
|
|
public SubsystemDrive(){
|
|
55
|
|
|
// assign initial values to CANTalons
|
|
56
|
|
|
// EX: left1 = new CANTalon(Constants.LEFT_MOTOR);
|
|
57
|
|
|
// call voltage for each instantiated CANTalon
|
|
58
|
|
|
// EX: voltage(left1);
|
|
59
|
|
|
// train each CANTalon
|
|
60
|
|
|
// master EX: left1.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative);
|
|
61
|
|
|
// left1.setEncPosition(0);
|
|
62
|
|
|
// left1.reverseSensor(false);
|
|
63
|
|
|
// slave EX: left2.changeControlMode(CANTalon.TalonControlMode.Follower);
|
|
64
|
|
|
// left2.set(left1.getDeviceID());
|
|
65
|
|
|
}
|
|
66
|
|
|
|
|
67
|
|
|
/** configures the voltage of each CANTalon */
|
|
68
|
|
|
private void voltage(CANTalon talon) {
|
|
69
|
|
|
talon.configNominalOutputVoltage(0f, 0f);
|
|
70
|
|
|
talon.configPeakOutputVoltage(12.0f, -12.0f);
|
|
|
|
|
|
|
71
|
|
|
talon.EnableCurrentLimit(true);
|
|
72
|
|
|
talon.setCurrentLimit(30);
|
|
|
|
|
|
|
73
|
|
|
}
|
|
74
|
|
|
|
|
75
|
|
|
|
|
76
|
|
|
|
|
77
|
|
|
|
|
78
|
|
|
|
|
79
|
|
|
|
|
80
|
|
|
/// EVERYTHING BELOW IS DRIFT_DRIVE SHELL CODE
|
|
81
|
|
|
|
|
82
|
|
|
|
|
83
|
|
|
/** method called by ManualCommandDrive
|
|
84
|
|
|
*
|
|
85
|
|
|
* @param joy
|
|
86
|
|
|
*/
|
|
87
|
|
|
public void driftDrive(Joystick joy) {
|
|
88
|
|
|
double joyVal = Xbox.LEFT_X(joy);
|
|
89
|
|
|
if (!IN_DRIFT) { // initiate drift if it hasn't been initiated already
|
|
90
|
|
|
IN_DRIFT = true;
|
|
|
|
|
|
|
91
|
|
|
if (joyVal > 0) { DRIFT_IS_CW = true; }
|
|
|
|
|
|
|
92
|
|
|
}
|
|
93
|
|
|
if (DRIFT_IS_CW) { joyVal = -1 * joyVal; } // align joyVal to the write negative and positive values
|
|
94
|
|
|
|
|
95
|
|
|
|
|
96
|
|
|
ROT_SPEED = (Xbox.LT(joy) + Xbox.LT(joy)) * 10d;
|
|
|
|
|
|
|
97
|
|
|
Drift status = updateDriftStatus(joyVal);
|
|
98
|
|
|
switch(status) {
|
|
99
|
|
|
case DONUT:
|
|
100
|
|
|
ROT_RADIUS = (Xbox.LEFT_X(joy)) + Constants.DRIFT_DEADZONE; // put stick on a 0-0.9 scale
|
|
|
|
|
|
|
101
|
|
|
ROT_RADIUS = 1 - ROT_RADIUS; // flip it to a .1-1 scale, outermost being .1 and innermost being 1
|
|
|
|
|
|
|
102
|
|
|
ROT_RADIUS *= Constants.MAX_DRIFT_RADIUS; // apply the .1-1 scale to the max radius
|
|
|
|
|
|
|
103
|
|
|
OFFSET = (int) (Constants.MAX_DRIFT_OFFSET * Math.abs(Xbox.LEFT_X(joy))); /// sets offset for max on tight radius
|
|
|
|
|
|
|
104
|
|
|
driveMecanumRot(ROT_RADIUS, ROT_SPEED, OFFSET);
|
|
105
|
|
|
break;
|
|
106
|
|
|
case DEAD:
|
|
107
|
|
|
// this will just continue the last known radius, offset, and speed
|
|
108
|
|
|
break;
|
|
109
|
|
|
case POWERSLIDE:
|
|
110
|
|
|
driveAngle(ROT_SPEED, 90 - OFFSET);
|
|
|
|
|
|
|
111
|
|
|
break;
|
|
112
|
|
|
case TURNOVER:
|
|
113
|
|
|
DRIFT_IS_CW = !DRIFT_IS_CW;
|
|
|
|
|
|
|
114
|
|
|
break;
|
|
115
|
|
|
}
|
|
116
|
|
|
}
|
|
117
|
|
|
|
|
118
|
|
|
/** moved from inside driftDrive; sets up the status switch in driftDrive */
|
|
119
|
|
|
private Drift updateDriftStatus(double joy) {
|
|
120
|
|
|
Drift status;
|
|
121
|
|
|
if (joy < (Constants.DRIFT_DEADZONE * -1)) { status = Drift.DONUT; }
|
|
122
|
|
|
else if (joy < (Constants.DRIFT_DEADZONE)) { status = Drift.DEAD; }
|
|
123
|
|
|
else if (joy < (Constants.DRIFT_TURNOVER)) { status = Drift.POWERSLIDE; }
|
|
124
|
|
|
else { status = Drift.TURNOVER; }
|
|
125
|
|
|
return status;
|
|
126
|
|
|
}
|
|
127
|
|
|
|
|
128
|
|
|
|
|
129
|
|
|
/**
|
|
130
|
|
|
* drives in an arc, partially strafing
|
|
131
|
|
|
* negative radius means left, positive radius means right
|
|
132
|
|
|
* (these still apply going backwards)
|
|
133
|
|
|
* speed is 0-10.0
|
|
134
|
|
|
* off is the degrees offset the front of the bot is
|
|
135
|
|
|
* (negative offset is pointed out, positive is pointed in)
|
|
136
|
|
|
* */
|
|
137
|
|
|
private void driveMecanumRot(double radius, double speed, int offset) {
|
|
138
|
|
|
CANTalon FL = null, BL = null, FR = null, BR = null; // remove when CANTalons are instantiated
|
|
|
|
|
|
|
139
|
|
|
|
|
140
|
|
|
double leftRotPower, rightRotPower;
|
|
|
|
|
|
|
141
|
|
|
if (radius < 0) { // if turning left, give more power to right side
|
|
142
|
|
|
rightRotPower = 1;
|
|
143
|
|
|
leftRotPower = calculateInnerRotPower(radius, Constants.DISTANCE_BETWEEN_WHEELS); }
|
|
144
|
|
|
else { // if turning right, give more power to left side
|
|
145
|
|
|
rightRotPower = calculateInnerRotPower(radius, Constants.DISTANCE_BETWEEN_WHEELS);
|
|
146
|
|
|
leftRotPower = 1; }
|
|
147
|
|
|
|
|
148
|
|
|
/// standard mecanum code (but the front wheels are averaged with the values for rotation)
|
|
149
|
|
|
FL.set((
|
|
|
|
|
|
|
150
|
|
|
Math.toRadians(
|
|
151
|
|
|
Math.cos(90 - (double) offset)
|
|
|
|
|
|
|
152
|
|
|
* speed)
|
|
153
|
|
|
+ leftRotPower)
|
|
154
|
|
|
/ 2);
|
|
|
|
|
|
|
155
|
|
|
FR.set((
|
|
|
|
|
|
|
156
|
|
|
Math.toRadians(
|
|
157
|
|
|
Math.sin(90 - (double) offset)
|
|
|
|
|
|
|
158
|
|
|
* speed)
|
|
159
|
|
|
+ rightRotPower)
|
|
160
|
|
|
/ 2);
|
|
|
|
|
|
|
161
|
|
|
BL.set(Math.toRadians(Math.sin(90 - (double) offset)) * speed);
|
|
|
|
|
|
|
162
|
|
|
BR.set(Math.toRadians(Math.cos(90 - (double) offset)) * speed);
|
|
|
|
|
|
|
163
|
|
|
}
|
|
164
|
|
|
|
|
165
|
|
|
/**
|
|
166
|
|
|
* strafes/moves at an angle
|
|
167
|
|
|
* speed should be -1.00 to 1.00
|
|
168
|
|
|
* offset must be -90 to 90 degrees
|
|
169
|
|
|
* */
|
|
170
|
|
|
private void driveAngle(double speed, int angle) { // right = 0, up = 90, left = 180, down = 270
|
|
171
|
|
|
CANTalon FL = null, BL = null, FR = null, BR = null; // remove when CANTalons are instantiated
|
|
|
|
|
|
|
172
|
|
|
|
|
173
|
|
|
/// standard mecanum strafing code
|
|
174
|
|
|
FL.set(Math.toRadians(Math.cos(angle)) * speed);
|
|
|
|
|
|
|
175
|
|
|
FR.set(Math.toRadians(Math.sin(angle)) * speed);
|
|
|
|
|
|
|
176
|
|
|
BL.set(Math.toRadians(Math.sin(angle)) * speed);
|
|
|
|
|
|
|
177
|
|
|
BR.set(Math.toRadians(Math.cos(angle)) * speed);
|
|
|
|
|
|
|
178
|
|
|
|
|
179
|
|
|
}
|
|
180
|
|
|
|
|
181
|
|
|
/** calculates inner power in roation, outer being max */
|
|
182
|
|
|
private double calculateInnerRotPower(double radius, double botWidth) {
|
|
183
|
|
|
return radius / (radius + botWidth);
|
|
184
|
|
|
}
|
|
185
|
|
|
|
|
186
|
|
|
}
|
|
187
|
|
|
|
|
188
|
|
|
|
wh1ter0se /
PowerUp-2018
Loading history...
See this CWE advisory on why this is a security issue.