Completed
Push — master ( 3f50c5...1672ec )
by John
22s
created

updateDriftStatus(double)   A

Complexity

Conditions 4

Size

Total Lines 7

Duplication

Lines 0
Ratio 0 %

Importance

Changes 0
Metric Value
dl 0
loc 7
rs 9.2
c 0
b 0
f 0
cc 4
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
0 ignored issues
show
Security introduced by
public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making OFFSET final.

See this CWE advisory on why this is a security issue.

Loading history...
18
	                      METADIRECTION; // on button down, this resets to 0; the angle relative to that initial angle (changes on physical rotation)
0 ignored issues
show
Security introduced by
public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making METADIRECTION final.

See this CWE advisory on why this is a security issue.

Loading history...
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring METADIRECTION on a separate line.
Loading history...
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)
0 ignored issues
show
Security introduced by
public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making ROT_SPEED final.

See this CWE advisory on why this is a security issue.

Loading history...
21
						  ROT_RADIUS;    // turn radius in inches
0 ignored issues
show
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring ROT_RADIUS on a separate line.
Loading history...
Security introduced by
public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making ROT_RADIUS final.

See this CWE advisory on why this is a security issue.

Loading history...
22
	public static boolean IN_DRIFT,		 // if the bot is currently in a drift
0 ignored issues
show
Security introduced by
public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making IN_DRIFT final.

See this CWE advisory on why this is a security issue.

Loading history...
23
						  DRIFT_IS_CW;   // if the drift is turning right
0 ignored issues
show
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring DRIFT_IS_CW on a separate line.
Loading history...
Security introduced by
public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making DRIFT_IS_CW final.

See this CWE advisory on why this is a security issue.

Loading history...
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; }
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 60.0 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
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; }
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 60.0 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
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);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 12.0f to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
71
    	talon.EnableCurrentLimit(true);
72
    	talon.setCurrentLimit(30);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 30 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
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;
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
91
    		if (joyVal > 0) { DRIFT_IS_CW = true; }
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
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;
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
Comprehensibility introduced by
Consider assigning this magic number 10d to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
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
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
101
				 				ROT_RADIUS  = 1 - ROT_RADIUS; // flip it to a .1-1 scale, outermost being .1 and innermost being 1
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
102
				 				ROT_RADIUS *= Constants.MAX_DRIFT_RADIUS; // apply the .1-1 scale to the max radius
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
103
			 				OFFSET = (int) (Constants.MAX_DRIFT_OFFSET * Math.abs(Xbox.LEFT_X(joy))); /// sets offset for max on tight radius
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
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);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 90 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
111
				 			break;
112
			 case TURNOVER:			
113
				 			DRIFT_IS_CW = !DRIFT_IS_CW;
0 ignored issues
show
Bug Multi Threading introduced by
Instance methods writing to static fields may lead to concurrency problems. Consider making the enclosing method static or removing this assignment to a static field.

If you really need to set this static field, consider writing a thread-safe setter and atomic getter.

Loading history...
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
0 ignored issues
show
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring FR on a separate line.
Loading history...
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring BL on a separate line.
Loading history...
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring BR on a separate line.
Loading history...
139
    	
140
    	double leftRotPower, rightRotPower;
0 ignored issues
show
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring rightRotPower on a separate line.
Loading history...
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(( 
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as FL appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
150
	    			Math.toRadians(
151
	    			Math.cos(90 - (double) offset)
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 90 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
152
	    			* speed) 
153
	    			+ leftRotPower) 
154
	    			/ 2);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 2 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
155
	    	FR.set((
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as FR appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
156
	    			Math.toRadians(
157
					Math.sin(90 - (double) offset) 
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 90 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
158
					* speed) 
159
					+ rightRotPower) 
160
					/ 2);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 2 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
161
	    	BL.set(Math.toRadians(Math.sin(90 - (double) offset)) * speed);
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as BL appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
Comprehensibility introduced by
Consider assigning this magic number 90 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
162
	    	BR.set(Math.toRadians(Math.cos(90 - (double) offset)) * speed);
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as BR appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
Comprehensibility introduced by
Consider assigning this magic number 90 to a constant.

Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.

Loading history...
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
0 ignored issues
show
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring FR on a separate line.
Loading history...
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring BR on a separate line.
Loading history...
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring BL on a separate line.
Loading history...
172
    	
173
    	/// standard mecanum strafing code
174
			FL.set(Math.toRadians(Math.cos(angle)) * speed);
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as FL appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
175
			FR.set(Math.toRadians(Math.sin(angle)) * speed);
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as FR appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
176
			BL.set(Math.toRadians(Math.sin(angle)) * speed);
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as BL appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
177
			BR.set(Math.toRadians(Math.cos(angle)) * speed);
0 ignored issues
show
Security Bug introduced by
A NullPointerException might be thrown here as BR appears to be nullable. Consider if you want to check against null before calling a method on this object.
Loading history...
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