Test Failed
Push — master ( d9c046...9db1c9 )
by John
04:04 queued 01:56
created

driveForza(Joystick,double,double)   A

Complexity

Conditions 3

Size

Total Lines 26

Duplication

Lines 0
Ratio 0 %

Importance

Changes 0
Metric Value
c 0
b 0
f 0
dl 0
loc 26
rs 9.256
cc 3
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.util.Xbox;
6
7
import com.ctre.CANTalon;
8
import com.ctre.phoenix.motorcontrol.ControlMode;
9
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
10
11
import edu.wpi.first.wpilibj.Joystick;
12
import edu.wpi.first.wpilibj.command.Subsystem;
13
14
/** VROOM VROOM */
15
public class SubsystemDrive extends Subsystem {
16
	
17
	
18
	private TalonSRX left1;
19
	private TalonSRX left2;
20
	private TalonSRX right1;
21
	private TalonSRX right2;
22
23
	
24
	/** runs at robot boot */
25
    public void initDefaultCommand() {
26
    	setDefaultCommand(new ManualCommandDrive()); }
27
    
28
    
29
    /** converts RPM to inches per second */
30
    public static final double rpm2ips(double rpm) {
31
    	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...
32
    
33
    
34
    /** converts an inches per second number to RPM */
35
    public static final double ips2rpm(double ips) {
36
    	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...
37
    
38
    
39
    /** converts rotations to distance traveled in inches */
40
    public static final double rot2in(double rot) {
41
    	return rot * Constants.WHEEL_DIAMETER * Math.PI; }
42
    
43
    
44
    /** converts distance traveled in inches to rotations */
45
    public static final double in2rot(double in) {
46
    	return in / Constants.WHEEL_DIAMETER / Math.PI; }
47
    
48
    /** apply left motor invert */
49
    public static final double leftify(double left) {
50
		return left * (Constants.LEFT_MOTOR_INVERT ? -1.0 : 1.0);
51
	}
52
53
    /** apply right motor invert */
54
	public static final double rightify(double right) {
55
		return right * (Constants.RIGHT_MOTOR_INVERT ? -1.0 : 1.0);
56
	}
57
	
58
	/** gives birth to the CANTalons */
59
    public SubsystemDrive(){
60
    	// masters
61
	    	left1 = new TalonSRX(Constants.LEFT_MASTER);
62
	    	right1 = new TalonSRX(Constants.RIGHT_MASTER);
63
    	
64
    	// slaves
65
	    	left2 = new TalonSRX(Constants.LEFT_SLAVE);
66
	    	right2 = new TalonSRX(Constants.RIGHT_SLAVE);
67
	    	
68
	    	//voltage(left1);
69
	    	//voltage(left2);
70
	    	//voltage(right1);
71
	    	//voltage(right2);
72
    }
73
    
74
    /** simple rocket league drive code; independent rotation and acceleration */
75
    public void driveRLTank(Joystick joy) {
76
    	double adder = Xbox.RT(joy) - Xbox.LT(joy);
77
    	double left = adder + (Xbox.LEFT_X(joy) / 1.333333);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 1.333333 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...
78
    	double right = adder - (Xbox.LEFT_X(joy) / 1.333333);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 1.333333 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...
79
    	
80
    	//Quick Truncate
81
    	left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left));
82
    	right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right));
83
    	    	
84
    	left1.set(ControlMode.PercentOutput, leftify(left));
85
    		left2.set(ControlMode.PercentOutput, leftify(left));
86
    	right1.set(ControlMode.PercentOutput, rightify(right));
87
    		right2.set(ControlMode.PercentOutput, rightify(right));
88
    	
89
    }
90
    
91
    /** drive code where rotation is dependent on acceleration 
92
     * @param radius 0.00-1.00, 1 being zero radius and 0 being driving in a line */
93
    public void driveForza(Joystick joy, double ramp, double radius) {
94
    	double left = 0, 
95
    		   right = 0;
0 ignored issues
show
Coding Style introduced by
Declaring several variables on the same line makes your code hard to read. Consider delaring right on a separate line.
Loading history...
96
    	double acceleration = Xbox.RT(joy) - Xbox.LT(joy);
97
    	
98
    	if (Xbox.LEFT_X(joy) < 0) {
99
    		right = acceleration;
100
    		left = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius; 
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...
101
    	} else if (Xbox.LEFT_X(joy) > 0) {
102
    		left = acceleration;
103
    		right = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius; 
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...
104
    	} else {
105
    		left = acceleration;
106
    		right = acceleration;
107
    	}
108
    	
109
    	/// ramps
110
	    	left1.configOpenloopRamp(ramp, 0);
111
	    		left2.configOpenloopRamp(ramp, 0);
112
	    	right1.configOpenloopRamp(ramp, 0);
113
	    		right2.configOpenloopRamp(ramp, 0);
114
    	
115
	    left1.set(ControlMode.PercentOutput, leftify(left));
116
			left2.set(ControlMode.PercentOutput, leftify(left));
117
		right1.set(ControlMode.PercentOutput, rightify(right));
118
			right2.set(ControlMode.PercentOutput, rightify(right));
119
    }
120
    
121
    /** configures the voltage of each CANTalon */
122
    private void voltage(TalonSRX talon) {
0 ignored issues
show
Unused Code introduced by
Your method has more parameters than it evaluates. Consider removing talon.
Loading history...
123
    	// talon.configNominalOutputVoltage(0f, 0f);
124
    	// talon.configPeakOutputVoltage(12.0f, -12.0f);
125
    	// talon.enableCurrentLimit(true);
126
    	// talon.configContinuousCurrentLimit(35, 300);
127
    }
128
    
129
    
130
131
}
132
133