001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007 package edu.wpi.first.wpilibj;
008
009 import com.sun.squawk.*;
010 import edu.wpi.first.wpilibj.communication.FRCControl;
011 import edu.wpi.first.wpilibj.communication.UsageReporting;
012 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
013
014 /**
015 * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase class.
016 *
017 * The IterativeRobot class is intended to be subclassed by a user creating a robot program.
018 *
019 * This class is intended to implement the "old style" default code, by providing
020 * the following functions which are called by the main loop, startCompetition(), at the appropriate times:
021 *
022 * robotInit() -- provide for initialization at robot power-on
023 *
024 * init() functions -- each of the following functions is called once when the
025 * appropriate mode is entered:
026 * - DisabledInit() -- called only when first disabled
027 * - AutonomousInit() -- called each and every time autonomous is entered from another mode
028 * - TeleopInit() -- called each and every time teleop is entered from another mode
029 * - TestInit() -- called each and every time test mode is entered from anothermode
030 *
031 * Periodic() functions -- each of these functions is called iteratively at the
032 * appropriate periodic rate (aka the "slow loop"). The period of
033 * the iterative robot is synced to the driver station control packets,
034 * giving a periodic frequency of about 50Hz (50 times per second).
035 * - disabledPeriodic()
036 * - autonomousPeriodic()
037 * - teleopPeriodic()
038 * - testPeriodoc()
039 *
040 */
041 public class IterativeRobot extends RobotBase {
042
043 private final static boolean TRACE_LOOP_ALLOCATIONS = false; // master tracing switch
044 private final static boolean TRACE_LOOP_ALLOCATIONS_AFTER_INIT = true; // trace before or after all phases initialize
045
046 private boolean m_disabledInitialized;
047 private boolean m_autonomousInitialized;
048 private boolean m_teleopInitialized;
049 private boolean m_testInitialized;
050
051 /**
052 * Constructor for RobotIterativeBase
053 *
054 * The constructor initializes the instance variables for the robot to indicate
055 * the status of initialization for disabled, autonomous, and teleop code.
056 */
057 public IterativeRobot() {
058 // set status for initialization of disabled, autonomous, and teleop code.
059 m_disabledInitialized = false;
060 m_autonomousInitialized = false;
061 m_teleopInitialized = false;
062 m_testInitialized = false;
063 }
064
065 /**
066 * Provide an alternate "main loop" via startCompetition().
067 *
068 */
069 public void startCompetition() {
070 UsageReporting.report(UsageReporting.kResourceType_Framework, UsageReporting.kFramework_Iterative);
071
072 robotInit();
073
074 // tracing support:
075 final int TRACE_LOOP_MAX = 100;
076 int loopCount = TRACE_LOOP_MAX;
077 Object marker = null;
078 boolean didDisabledPeriodic = false;
079 boolean didAutonomousPeriodic = false;
080 boolean didTeleopPeriodic = false;
081 boolean didTestPeriodic = false;
082 if (TRACE_LOOP_ALLOCATIONS) {
083 GC.initHeapStats();
084 if (!TRACE_LOOP_ALLOCATIONS_AFTER_INIT) {
085 System.out.println("=== Trace allocation in competition loop! ====");
086 marker = new Object(); // start counting objects before any loop starts - includes class initialization
087 }
088 }
089
090 // loop forever, calling the appropriate mode-dependent function
091 LiveWindow.setEnabled(false);
092 while (true) {
093 if (TRACE_LOOP_ALLOCATIONS && didDisabledPeriodic && didAutonomousPeriodic && didTeleopPeriodic && --loopCount <= 0) {
094 System.out.println("!!!!! Stop loop!");
095 break;
096 }
097 // Call the appropriate function depending upon the current robot mode
098 if (isDisabled()) {
099 // call DisabledInit() if we are now just entering disabled mode from
100 // either a different mode or from power-on
101 if (!m_disabledInitialized) {
102 LiveWindow.setEnabled(false);
103 disabledInit();
104 m_disabledInitialized = true;
105 // reset the initialization flags for the other modes
106 m_autonomousInitialized = false;
107 m_teleopInitialized = false;
108 m_testInitialized = false;
109 }
110 if (nextPeriodReady()) {
111 FRCControl.observeUserProgramDisabled();
112 disabledPeriodic();
113 didDisabledPeriodic = true;
114 }
115 } else if (isTest()) {
116 // call TestInit() if we are now just entering test mode from either
117 // a different mode or from power-on
118 if (!m_testInitialized) {
119 LiveWindow.setEnabled(true);
120 testInit();
121 m_testInitialized = true;
122 m_autonomousInitialized = false;
123 m_teleopInitialized = false;
124 m_disabledInitialized = false;
125 }
126 if (nextPeriodReady()) {
127 FRCControl.observeUserProgramTest();
128 testPeriodic();
129 didTestPeriodic = true;
130 }
131 } else if (isAutonomous()) {
132 // call Autonomous_Init() if this is the first time
133 // we've entered autonomous_mode
134 if (!m_autonomousInitialized) {
135 LiveWindow.setEnabled(false);
136 // KBS NOTE: old code reset all PWMs and relays to "safe values"
137 // whenever entering autonomous mode, before calling
138 // "Autonomous_Init()"
139 autonomousInit();
140 m_autonomousInitialized = true;
141 m_testInitialized = false;
142 m_teleopInitialized = false;
143 m_disabledInitialized = false;
144 }
145 if (nextPeriodReady()) {
146 getWatchdog().feed();
147 FRCControl.observeUserProgramAutonomous();
148 autonomousPeriodic();
149 didAutonomousPeriodic = true;
150 }
151 } else {
152 // call Teleop_Init() if this is the first time
153 // we've entered teleop_mode
154 if (!m_teleopInitialized) {
155 LiveWindow.setEnabled(false);
156 teleopInit();
157 m_teleopInitialized = true;
158 m_testInitialized = false;
159 m_autonomousInitialized = false;
160 m_disabledInitialized = false;
161 }
162 if (nextPeriodReady()) {
163 getWatchdog().feed();
164 FRCControl.observeUserProgramTeleop();
165 teleopPeriodic();
166 didTeleopPeriodic = true;
167 }
168 }
169
170 if (TRACE_LOOP_ALLOCATIONS && TRACE_LOOP_ALLOCATIONS_AFTER_INIT &&
171 didDisabledPeriodic && didAutonomousPeriodic && didTeleopPeriodic && loopCount == TRACE_LOOP_MAX) {
172 System.out.println("=== Trace allocation in competition loop! ====");
173 marker = new Object(); // start counting objects after 1st loop completes - ignore class initialization
174 }
175 m_ds.waitForData();
176 }
177 if (TRACE_LOOP_ALLOCATIONS) {
178 GC.printHeapStats(marker, false);
179 }
180 }
181
182 /**
183 * Determine if the appropriate next periodic function should be called.
184 * Call the periodic functions whenever a packet is received from the Driver Station, or about every 20ms.
185 */
186 private boolean nextPeriodReady() {
187 return m_ds.isNewControlData();
188 }
189
190 /* ----------- Overridable initialization code -----------------*/
191
192 /**
193 * Robot-wide initialization code should go here.
194 *
195 * Users should override this method for default Robot-wide initialization which will
196 * be called when the robot is first powered on. It will be called exactly 1 time.
197 */
198 public void robotInit() {
199 System.out.println("Default IterativeRobot.robotInit() method... Overload me!");
200 }
201
202 /**
203 * Initialization code for disabled mode should go here.
204 *
205 * Users should override this method for initialization code which will be called each time
206 * the robot enters disabled mode.
207 */
208 public void disabledInit() {
209 System.out.println("Default IterativeRobot.disabledInit() method... Overload me!");
210 }
211
212 /**
213 * Initialization code for autonomous mode should go here.
214 *
215 * Users should override this method for initialization code which will be called each time
216 * the robot enters autonomous mode.
217 */
218 public void autonomousInit() {
219 System.out.println("Default IterativeRobot.autonomousInit() method... Overload me!");
220 }
221
222 /**
223 * Initialization code for teleop mode should go here.
224 *
225 * Users should override this method for initialization code which will be called each time
226 * the robot enters teleop mode.
227 */
228 public void teleopInit() {
229 System.out.println("Default IterativeRobot.teleopInit() method... Overload me!");
230 }
231
232 /**
233 * Initialization code for test mode should go here.
234 *
235 * Users should override this method for initialization code which will be called each time
236 * the robot enters test mode.
237 */
238 public void testInit() {
239 System.out.println("Default IterativeRobot.testInit() method... Overload me!");
240 }
241
242 /* ----------- Overridable periodic code -----------------*/
243
244 private boolean dpFirstRun = true;
245 /**
246 * Periodic code for disabled mode should go here.
247 *
248 * Users should override this method for code which will be called periodically at a regular
249 * rate while the robot is in disabled mode.
250 */
251 public void disabledPeriodic() {
252 if (dpFirstRun) {
253 System.out.println("Default IterativeRobot.disabledPeriodic() method... Overload me!");
254 dpFirstRun = false;
255 }
256 Timer.delay(0.001);
257 }
258
259 private boolean apFirstRun = true;
260
261 /**
262 * Periodic code for autonomous mode should go here.
263 *
264 * Users should override this method for code which will be called periodically at a regular
265 * rate while the robot is in autonomous mode.
266 */
267 public void autonomousPeriodic() {
268 if (apFirstRun) {
269 System.out.println("Default IterativeRobot.autonomousPeriodic() method... Overload me!");
270 apFirstRun = false;
271 }
272 Timer.delay(0.001);
273 }
274
275 private boolean tpFirstRun = true;
276
277 /**
278 * Periodic code for teleop mode should go here.
279 *
280 * Users should override this method for code which will be called periodically at a regular
281 * rate while the robot is in teleop mode.
282 */
283 public void teleopPeriodic() {
284 if (tpFirstRun) {
285 System.out.println("Default IterativeRobot.teleopPeriodic() method... Overload me!");
286 tpFirstRun = false;
287 }
288 Timer.delay(0.001);
289 }
290
291 private boolean tmpFirstRun = true;
292
293 /**
294 * Periodic code for test mode should go here
295 *
296 * Users should override this method for code which will be called periodically at a regular rate
297 * while the robot is in test mode.
298 */
299 public void testPeriodic() {
300 if (tmpFirstRun) {
301 System.out.println("Default IterativeRobot.testPeriodic() method... Overload me!");
302 tmpFirstRun = false;
303 }
304 }
305 }