Completed
Push — master ( 74c8db...6c2084 )
by John
33s
created

publishSwitches()   A

Complexity

Conditions 1

Size

Total Lines 5

Duplication

Lines 0
Ratio 0 %

Importance

Changes 0
Metric Value
cc 1
c 0
b 0
f 0
dl 0
loc 5
rs 10
1
package org.usfirst.frc.team3695.robot.subsystems;
2
3
import com.ctre.phoenix.motorcontrol.ControlMode;
4
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
5
import edu.wpi.first.wpilibj.DigitalInput;
6
import edu.wpi.first.wpilibj.Joystick;
7
import edu.wpi.first.wpilibj.command.Subsystem;
8
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
9
import org.usfirst.frc.team3695.robot.Constants;
10
import org.usfirst.frc.team3695.robot.Robot;
11
import org.usfirst.frc.team3695.robot.commands.ManualCommandGrow;
12
import org.usfirst.frc.team3695.robot.enumeration.Bot;
13
import org.usfirst.frc.team3695.robot.enumeration.Position;
14
import org.usfirst.frc.team3695.robot.util.Xbox;
15
16
/** the big, metal pole */
17
public class SubsystemMast extends Subsystem {
18
	
19
	
20
	private TalonSRX leftPinion;
21
	private TalonSRX rightPinion;
22
	private TalonSRX screw;
23
	
24
	DigitalInput lowerPinionLimit;
25
    DigitalInput upperPinionLimit;
26
    DigitalInput lowerScrewLimit;
27
    DigitalInput midScrewLimit;
28
    DigitalInput upperScrewLimit;
29
30
    private Boolean override;
31
32
	
33
	/** runs at robot boot */
34
    public void initDefaultCommand() {
35
    	setDefaultCommand(new ManualCommandGrow()); }
36
	
37
	/** gives birth to the CANTalons */
38
    public SubsystemMast(){
39
    	lowerPinionLimit = new DigitalInput(1);
40
        upperPinionLimit = new DigitalInput(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...
41
        lowerScrewLimit  = new DigitalInput(3);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 3 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
        upperScrewLimit  = new DigitalInput(4);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 4 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...
43
    	
44
    	leftPinion = new TalonSRX(Constants.LEFT_PINION_MOTOR);
45
    	rightPinion = new TalonSRX(Constants.RIGHT_PINION_MOTOR);
46
    	screw = new TalonSRX(Constants.SCREW_MOTOR);
47
    		voltage(leftPinion);
48
    		voltage(rightPinion);
49
    		voltage(screw);
50
    		
51
		override = false;
52
    }
53
    
54
    public void setOverride(Boolean override) {
55
    	this.override = override;
56
    }
57
58
   	/** apply pinion motor invert */
59
   	public static final double leftPinionate(double left) {
60
   		Boolean invert = Robot.bot == Bot.OOF ? Constants.OOF.LEFT_PINION_MOTOR_INVERT : Constants.SWISS.LEFT_PINION_MOTOR_INVERT;
61
   		return left * (invert ? -1.0 : 1.0);
62
   	}
63
   	
64
   	/** apply screw motor invert */
65
   	public static final double rightPinionate(double right) {
66
   		Boolean invert = Robot.bot == Bot.OOF ? Constants.OOF.RIGHT_PINION_MOTOR_INVERT : Constants.SWISS.RIGHT_PINION_MOTOR_INVERT;
67
   		return right * (invert ? -1.0 : 1.0);
68
   	}
69
   	
70
   	public static final double screwify(double screw) {
71
   		Boolean invert = Robot.bot == Bot.OOF ? Constants.OOF.SCREW_MOTOR_INVERT : Constants.SWISS.SCREW_MOTOR_INVERT;
72
   		return screw * (invert ? -1.0 : 1.0);
73
   	}
74
    
75
   	/** raise the mast at RT-LR trigger speed */
76
    public void moveBySpeed(Joystick joy, double inhibitor) {
77
    	double dualAction = Xbox.RT(joy) - Xbox.LT(joy);
78
    	double screwSpeed = Xbox.LEFT_Y(joy) + dualAction;
79
    	double pinionSpeed = Xbox.RIGHT_Y(joy) + dualAction;
80
    	
81
		if (!lowerPinionLimit.get() && pinionSpeed > 0)   { pinionSpeed = 0; }
82
		if (!upperPinionLimit.get() && pinionSpeed < 0)   { pinionSpeed = 0; }
83
		if (!lowerScrewLimit.get()  && screwSpeed  > 0)   { screwSpeed = 0;  }
84
		if (!upperScrewLimit.get()  && screwSpeed  < 0)   { screwSpeed = 0;  }
85
			
86
    	publishSwitches();
87
    	if (!override) {
88
    		leftPinion.set(ControlMode.PercentOutput, leftPinionate(pinionSpeed));
89
        	rightPinion.set(ControlMode.PercentOutput, rightPinionate(pinionSpeed));
90
        	screw.set(ControlMode.PercentOutput, inhibitor * screwify(screwSpeed));
91
    	}
92
    }
93
    
94
    public Boolean dropIt() {
95
    	if (lowerPinionLimit.get() && upperPinionLimit.get()) {
96
	    	leftPinion.set(ControlMode.PercentOutput, leftPinionate(-1));
97
	    	rightPinion.set(ControlMode.PercentOutput, rightPinionate(-1));
98
    	} else {
99
    		leftPinion.set(ControlMode.PercentOutput, 0);
100
	    	rightPinion.set(ControlMode.PercentOutput, 0);
101
    	}
102
    	
103
    	if (lowerScrewLimit.get() && upperScrewLimit.get()) {
104
    		screw.set(ControlMode.PercentOutput, screwify(1));
105
    	} else {
106
    		screw.set(ControlMode.PercentOutput, 0);
107
    	}
108
    	
109
    	return (!lowerPinionLimit.get() || !upperPinionLimit.get()) && (!lowerScrewLimit.get() || !upperScrewLimit.get());
110
    }
111
112
    public void adjustPinion(Position direction){
113
114
    	if (direction == Position.UP) {
115
			while (!upperPinionLimit.get()) {
116
				rightPinion.set(ControlMode.PercentOutput, rightPinionate(1));
117
				leftPinion.set(ControlMode.PercentOutput, leftPinionate(1));
118
			}
119
		} else if (direction == Position.DOWN) {
120
			while (!lowerPinionLimit.get()) {
121
				rightPinion.set(ControlMode.PercentOutput, rightPinionate(-1));
122
				leftPinion.set(ControlMode.PercentOutput, leftPinionate(-1));
123
			}
124
		}
125
		rightPinion.set(ControlMode.PercentOutput, rightPinionate(0));
126
    	leftPinion.set(ControlMode.PercentOutput, leftPinionate(0));
127
	}
128
129
	public void adjustScrew(Position direction){
130
    	if (direction == Position.UP){
131
    		while (!upperScrewLimit.get()){
132
    			screw.set(ControlMode.PercentOutput, screwify(1));
133
			}
134
		} else if (direction == Position.DOWN){
135
    		while (!lowerScrewLimit.get()){
136
    			screw.set(ControlMode.PercentOutput, screwify(-1));
137
			}
138
		}
139
		screw.set(ControlMode.PercentOutput, screwify(0));
140
	}
141
    	
142
    public void publishSwitches() {
143
    	SmartDashboard.putBoolean("Lower Screw", !lowerScrewLimit.get());
144
    	SmartDashboard.putBoolean("Upper Screw", !upperScrewLimit.get());
145
    	SmartDashboard.putBoolean("Lower Pinion", !lowerPinionLimit.get());
146
    	SmartDashboard.putBoolean("Upper Pinion", !upperPinionLimit.get());
147
    }
148
    
149
    /** configures the voltage of each CANTalon */
150
    private void voltage(TalonSRX talon) {
151
    	// talon.configNominalOutputVoltage(0f, 0f);
152
    	// talon.configPeakOutputVoltage(12.0f, -12.0f);
153
    	talon.enableCurrentLimit(true);
154
    	talon.configContinuousCurrentLimit(60, 300);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 60 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
    	talon.configPeakCurrentDuration(500, 10);
0 ignored issues
show
Comprehensibility introduced by
Consider assigning this magic number 500 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...
156
    }
157
    
158
    
159
160
}
161
162