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
|
package org.usfirst.frc.team4272.robot2015;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintWriter;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends IterativeRobot {
private final HwRobot robot = new HwRobot();
private final HwOI oi = new HwOI();
private final DriverStation ds = DriverStation.getInstance();
private final Control control = new Control();
private PrintWriter log;
private Autonomous auto;
private Teleop teleop;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() {
try {
inLog();
} catch (Exception e) {}
}
private void inLog() throws IOException {
log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
log.println("\ntime"
+",ds:alliance,ds:voltage,ds:FMStime"
+",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push"
+",i:lDrive.dist,i:lDrive.rate"
+",i:rDrive.dist,i:rDrive.rate"
+",i:winch.dist");
}
private void doLog() throws Exception {
SmartDashboard.putNumber("dist", robot.rDriveE.getDistance());
SmartDashboard.putNumber("raw", robot.rDriveE.getRaw());
if (!ds.isFMSAttached())
return;
log.println(0
+","+ds.getAlliance()
+","+ds.getBatteryVoltage()
+","+ds.getMatchTime()
+","+control.lDrive
+","+control.rDrive
+","+control.winch
+","+fmtRelay(control.lIntake)
+","+fmtRelay(control.rIntake)
+","+control.grab
+","+control.push
+","+robot.lDriveE.getDistance()
+","+robot.lDriveE.getRate()
+","+robot.rDriveE.getDistance()
+","+robot.rDriveE.getRate()
+","+robot.winchE.getRate()
);
}
private String fmtRelay(Relay.Value v) {
if (v == Relay.Value.kForward) {
return "kForward";
} else if (v == Relay.Value.kReverse) {
return "kReverse";
} else if (v == Relay.Value.kOn) {
return "kOn";
} else if (v == Relay.Value.kOff) {
return "kOff";
} else {
return "unknown";
}
}
public void autonomousInit() {
auto = new Autonomous(robot);
}
public void autonomousPeriodic() {
try {
robot.run(auto.run(control));
doLog();
} catch (Exception e) {}
}
public void teleopInit() {
robot.rDriveE.reset();
teleop = new Teleop();
}
public void teleopPeriodic() {
try {
robot.run(teleop.run(control, oi));
doLog();
} catch (Exception e) {}
}
public void disabledInit() {
}
public void disabledPeriodic() {
}
/**
* This function is called periodically during test mode
*/
public void testPeriodic() {
}
}
|
