/
TeleopControl.java
151 lines (133 loc) · 5.25 KB
/
TeleopControl.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
package org.lunatecs316.frc2014;
import edu.wpi.first.wpilibj.Joystick;
import org.lunatecs316.frc2014.lib.EnhancedJoystick;
import org.lunatecs316.frc2014.lib.Logger;
import org.lunatecs316.frc2014.lib.Util;
import org.lunatecs316.frc2014.lib.XboxController;
import org.lunatecs316.frc2014.subsystems.Drivetrain;
import org.lunatecs316.frc2014.subsystems.Pickup;
import org.lunatecs316.frc2014.subsystems.Shooter;
/**
* Manages control of the Robot during Teleop mode
* @author Christian Steward
* @author Domenic Rodriguez
*/
public class TeleopControl {
private XboxController driverController = new XboxController(RobotMap.DriverController);
private EnhancedJoystick operatorJoystick = new EnhancedJoystick(RobotMap.OperatorJoystick);
private Drivetrain drivetrain = Drivetrain.getInstance();
private Pickup pickup = Pickup.getInstance();
private Shooter shooter = Shooter.getInstance();
private boolean cheesyMode;
/**
* Setup for Teleop mode
*/
public void init() {
drivetrain.setSafetyEnabled(false);
cheesyMode = true;
}
/**
* Run one iteration of Teleop mode
*/
public void run() {
updateJoysticks();
// Driving
if (driverController.getButtonPressed(XboxController.ButtonA)) {
if (cheesyMode) {
drivetrain.setSafetyEnabled(true);
Logger.info("TeleopControl#run", "Switching to Arcade Drive Mode");
cheesyMode = false;
} else {
Logger.info("TeleopControl#run", "Switching to Cheesy Drive Mode");
drivetrain.setSafetyEnabled(false);
cheesyMode = true;
}
}
if (driverController.getButton(XboxController.ButtonB)) {
drivetrain.driveStraight(-0.5);
} else if (driverController.getButton(XboxController.ButtonX)) {
drivetrain.driveStraightDistance(Constants.DrivetrainSetpoint.getValue());
} else if (driverController.getButton(XboxController.ButtonY)) {
drivetrain.resetGyro();
drivetrain.resetEncoders();
} else {
double move = Util.deadband(driverController.getLeftY(), Constants.JoystickDeadband.getValue());
double turn = Util.deadband(driverController.getRightX(), Constants.JoystickDeadband.getValue());
if (cheesyMode)
drivetrain.cheesyDrive(move, turn);
else
drivetrain.arcadeDrive(move, turn);
}
// Shifting
if (driverController.getButton(XboxController.RightBumper))
drivetrain.shiftUp();
else
drivetrain.shiftDown();
// Catching Aid
if (driverController.getButtonPressed(XboxController.LeftBumper))
drivetrain.toggleCatchingAid();
// Pickup Position
if (operatorJoystick.getButtonPressed(11) || operatorJoystick.getButtonPressed(4))
pickup.raise();
else if (operatorJoystick.getButtonPressed(5)) //button 5 be broke
pickup.lower();
// Pickup Rollers
double rollerSpeed = ((0.25 * -operatorJoystick.getZ()) + 0.75);
if (operatorJoystick.getButton(7))
pickup.setRollerSpeed(-rollerSpeed);
else if (operatorJoystick.getButton(6))
pickup.setRollerSpeed(rollerSpeed);
else
pickup.stopRollers();
// Shooter
if (operatorJoystick.getButtonPressed(1) && ((pickup.isLowered() && shooter.ballIsLoaded()) || SamXV.manualOverride())) {
logShot();
shooter.fire();
} else if (operatorJoystick.getButton(2)) {
shooter.setPosition(Constants.StaticShooterSetpoint.getValue());
} else if (operatorJoystick.getButton(9)) {
shooter.setPosition(Constants.TrussShotSetpoint.getValue());
} else if (operatorJoystick.getButton(3)) {
shooter.autoAim(drivetrain.getRangeFinderDistance());
} else if (operatorJoystick.getButtonPressed(8)) {
shooter.reload();
} else {
double value = Util.deadband(operatorJoystick.getY(), Constants.JoystickDeadband.getValue());
if (shooter.isManualControl() || operatorJoystick.getRawButton(10))
shooter.setWinch(value);
}
}
/**
* Read the buttons on the joysticks. Required before using
* edge detection methods.
*/
public void updateJoysticks() {
driverController.update();
operatorJoystick.update();
}
/**
* Get the driver's joystick
* @return the driver controller
*/
public XboxController getDriverController() {
return driverController;
}
/**
* Get the operator's joystick
* @return the operator joystick
*/
public Joystick getOperatorJoystick() {
return operatorJoystick;
}
/**
* Record information while shooting
*/
public void logShot() {
double distance = drivetrain.getRangeFinderDistance();
double angle = drivetrain.getGyroAngle();
double armPosition = shooter.getArmPosition();
Logger.enableFileLogging(true);
Logger.debug("logShot", "ArmPos: " + armPosition + " Dist: " + distance + " GyroAngle: " + angle);
Logger.enableFileLogging(false);
}
}