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
|
package org.usfirst.frc.team4272.robot2016;
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.util.Date;
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("/home/lvuser/tweeterbot/var/data.log", true));
log.println("\ntime"
+",ds:FMS,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 doLogOnce(String tag) throws Exception {
log.println(new Date()
+","+ds.isFMSAttached()
+","+tag);
}
private void doLog() throws Exception {
SmartDashboard.putNumber("dist", robot.rDriveE.getDistance());
SmartDashboard.putNumber("raw", robot.rDriveE.getRaw());
if (!ds.isFMSAttached())
return;
log.println(new Date()
+","+ds.isFMSAttached()
+","+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() {
try {
auto = new Autonomous(robot);
doLogOnce("autonomous");
} catch (Exception e) {}
}
public void autonomousPeriodic() {
try {
robot.run(auto.run(control));
doLog();
} catch (Exception e) {}
}
public void teleopInit() {
try {
robot.rDriveE.reset();
teleop = new Teleop();
doLogOnce("teleop");
} catch (Exception e) {}
}
public void teleopPeriodic() {
try {
robot.run(teleop.run(control, oi));
doLog();
} catch (Exception e) {}
}
public void disabledInit() {
try {
doLogOnce("disabled");
log.flush();
} catch (Exception e) {}
}
public void disabledPeriodic() {
}
/**
* This function is called periodically during test mode
*/
public void testPeriodic() {
}
}
|
