-
Notifications
You must be signed in to change notification settings - Fork 1
/
Autonomous.java
107 lines (100 loc) · 5.07 KB
/
Autonomous.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
package org.usfirst.frc.team3464.robot.commands;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.command.CommandGroup;
import java.util.function.DoubleSupplier;
import org.usfirst.frc.team3464.robot.RobotMap;
public class Autonomous extends CommandGroup { // All of the auto presets
private char start;
public Autonomous(char start) { // Read starting position from GUI
this.start = start;
}
@Override
public void start() { // Read field layout and map start and end points to movement paths
//DoubleSupplier driveProgress = RobotMap.timer::get;
DoubleSupplier driveProgress = RobotMap.leftEncoder::getDistance;
char fms = '_';
fms = DriverStation.getInstance().getGameSpecificMessage().charAt(0);
addSequential(new Grab(true));
addParallel(new CubeLift(RobotMap.top));
if ((start == 'L' || start == 'l') && fms == 'L') { // Move down left lane
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 124.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, 90.0));
addSequential(new Drive(RobotMap.timer::get, 2.0, (t) -> .6));
addSequential(new Grab(false));
}
else if (start == 'L' && fms == 'R') { // Cross lanes and drop the cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 180.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, 90.0));
addSequential(new Drive(driveProgress, 130.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, 45.0));
addSequential(new Drive(RobotMap.timer::get, .25, (t) -> .6));
addSequential(new Turn(RobotMap.gyro::getAngle, 45.0));
addSequential(new Drive(RobotMap.timer::get, .25, (t) -> .6));
addSequential(new Grab(false));
}
else if (start == 'M' && fms == 'L') { // Adjust angle and drop the cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .85));
addSequential(new Drive(driveProgress, 5.0, (t) -> .85));
addSequential(new Turn(RobotMap.gyro::getAngle, -45.0));
addSequential(new Drive(driveProgress, /**99.0**/ 60.0, (t) -> .70));
addSequential(new Turn(RobotMap.gyro::getAngle, 45.0));
addSequential(new Drive(RobotMap.timer::get, 2.0, (t) -> .60));
addSequential(new Grab(false));
}
else if (start == 'M' && fms == 'R') { // Adjut angle and drop the cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .85));
addSequential(new Drive(driveProgress, 5.0, (t) -> .85));
addSequential(new Turn(RobotMap.gyro::getAngle, 45.0));
addSequential(new Drive(driveProgress, 60.0, (t) -> .70));
addSequential(new Turn(RobotMap.gyro::getAngle, -45.0));
addSequential(new Drive(RobotMap.timer::get, 2.0, (t) -> .60));
addSequential(new Grab(false));
}
else if (start == 'R' && fms == 'L') { // Cross lanes and drop the cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 180.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, -90.0));
addSequential(new Drive(driveProgress, 130.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, -45.0));
addSequential(new Drive(RobotMap.timer::get, .25, (t) -> .6));
addSequential(new Turn(RobotMap.gyro::getAngle, -45.0));
addSequential(new Drive(RobotMap.timer::get, .25, (t) -> .6));
addSequential(new Grab(false));
}
else if ((start =='R' || start == 'r') && fms == 'R') { // Cross lanes, keep cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 124.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, -90.0));
addSequential(new Drive(RobotMap.timer::get, 2.0, (t) -> .6));
addSequential(new Grab(false));
}
else if (start == 'l' && fms == 'R') { // Cross lanes, keep cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 180.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, 90.0));
addSequential(new Drive(driveProgress, 80.0, (t) -> .60));
}
else if (start == 'r' && fms == 'L') { // Cross lanes, keep cube
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 5.0, (t) -> .65));
addSequential(new Drive(driveProgress, 180.0, (t) -> .65));
addSequential(new Turn(RobotMap.gyro::getAngle, -90.0));
addSequential(new Drive(driveProgress, 80.0, (t) -> .60));
}
else { // Enter field and wait
addSequential(new Drive(RobotMap.timer::get, 2.0, (t -> .5)));
}
super.start();
}
@Override
protected boolean isFinished() { // Stop gracefully when auto is over, but tele will override this if it needs to anyways
return DriverStation.getInstance().isAutonomous() == false;
}
}