TRC Library for FRC (2011) 2011
TRC Library Code Project

C:/Users/Michael/Frc/2011/code/trclib/TrcRobot.h

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 #endif
00014 
00015 #ifndef _TRCROBOT_H
00016 #define _TRCROBOT_H
00017 
00018 #ifdef MOD_ID
00019     #undef MOD_ID
00020 #endif
00021 #define MOD_ID                  MOD_TRCROBOT
00022 #ifdef MOD_NAME
00023     #undef MOD_NAME
00024 #endif
00025 #define MOD_NAME                "TrcRobot"
00026 
00033 class TrcRobot: public RobotBase
00034 {
00035 private:
00036     bool    m_DisabledInitialized;
00037     bool    m_AutonomousInitialized;
00038     bool    m_TeleopInitialized;
00039     double  m_LoopPeriod;
00040     Timer   m_LoopTimer;
00041     UINT32  m_periodPacket;     //in ms
00042 
00053     bool
00054     NextPeriodReady(
00055         void
00056         )
00057     {
00058         bool rc;
00059 
00060         TLevel(HIFREQ);
00061         TEnter();
00062 
00063         static UINT32 timestamp = 0;
00064         if (m_LoopPeriod > 0.0)
00065         {
00066             rc = m_LoopTimer.HasPeriodPassed(m_LoopPeriod);
00067         }
00068         else
00069         {
00070             rc = m_ds->IsNewControlData();
00071             if (rc)
00072             {
00073                 //
00074                 // Determine the packet interval.
00075                 //
00076                 UINT32 timeCurr = GetMsecTime();
00077                 m_periodPacket = timeCurr - timestamp;
00078                 timestamp = timeCurr;
00079             }
00080         }
00081 
00082         TExitMsg(("=%d", rc));
00083         return rc;
00084     }   //NextPeriodReady
00085 
00086 public:
00087     /*
00088      * The default period for the periodic function calls (seconds)
00089      * Setting the period to 0.0 will cause the periodic functions to
00090      * follow the Driver Station packet rate of about 50Hz.
00091      */
00092     static const double kDefaultPeriod = 0.0;
00093 
00097     virtual
00098     void
00099     RobotInit(
00100         void
00101         )
00102     {
00103         TLevel(INIT);
00104         TEnter();
00105         TExit();
00106     }   //RobotInit
00107 
00111     virtual
00112     void
00113     DisabledInit(
00114         void
00115         )
00116     {
00117         TLevel(INIT);
00118         TEnter();
00119         TExit();
00120     }   //DisabledInit
00121 
00125     virtual
00126     void
00127     AutonomousInit(
00128         void
00129         )
00130     {
00131         TLevel(INIT);
00132         TEnter();
00133         TExit();
00134     }   //AutonomousInit
00135 
00139     virtual
00140     void
00141     TeleopInit(
00142         void
00143         )
00144     {
00145         TLevel(INIT);
00146         TEnter();
00147         TExit();
00148     }   //TeleopInit
00149 
00154     virtual
00155     void
00156     DisabledPeriodic(
00157         void
00158         )
00159     {
00160         TLevel(HIFREQ);
00161         TEnter();
00162         TExit();
00163     }   //DisabledPeriodic
00164 
00169     virtual
00170     void
00171     AutonomousPeriodic(
00172         void
00173         )
00174     {
00175         TLevel(HIFREQ);
00176         TEnter();
00177         TExit();
00178     }   //AutonomousPeriodic
00179 
00183     virtual
00184     void
00185     TeleopPeriodic(
00186         void
00187         )
00188     {
00189         TLevel(HIFREQ);
00190         TEnter();
00191         TExit();
00192     }   //TeleopPeriodic
00193 
00197     virtual
00198     void
00199     DisabledContinuous(
00200         void
00201         )
00202     {
00203         TLevel(HIFREQ);
00204         TEnter();
00205         //
00206         // Yield to other threads so they don't starve.
00207         //
00208         Wait(GetPeriod()/10.0);
00209         TExit();
00210     }   //DisabledContinuous
00211 
00215     virtual
00216     void
00217     AutonomousContinuous(
00218         void
00219         )
00220     {
00221         TLevel(HIFREQ);
00222         TEnter();
00223         //
00224         // Yield to other threads so they don't starve.
00225         //
00226         Wait(GetPeriod()/10.0);
00227         TExit();
00228     }   //AutonomousContinuous
00229 
00233     virtual
00234     void
00235     TeleopContinuous(
00236         void
00237         )
00238     {
00239         TLevel(HIFREQ);
00240         TEnter();
00241         //
00242         // Yield to other threads so they don't starve.
00243         //
00244         Wait(GetPeriod()/10.0);
00245         TExit();
00246     }   //TeleopContinuous
00247 
00254     void
00255     SetPeriod(
00256         __in double period
00257         )
00258     {
00259         TLevel(API);
00260         TEnterMsg(("period=%f", period));
00261 
00262         if (period != 0.0)
00263         {
00264             //
00265             // Not sync with DS, so start the timer for the main loop.
00266             //
00267             m_LoopTimer.Reset();
00268             m_LoopTimer.Start();
00269         }
00270         else
00271         {
00272             //
00273             // Sync with DS, don't need timer.
00274             //
00275             m_LoopTimer.Stop();
00276         }
00277         m_LoopPeriod = period;
00278 
00279         TExit();
00280     }   //SetPeriod
00281     
00287     double
00288     GetPeriod(
00289         void
00290         )
00291     {
00292         TLevel(HIFREQ);
00293         TEnter();
00294         double period = (m_LoopPeriod == 0.0)?
00295                         ((double)m_periodPacket)/1000.0: m_LoopPeriod;
00296         TExitMsg(("=%f", period));
00297         return period;
00298     }   //GetPeriod
00299 
00306     double
00307     GetLoopsPerSec(
00308         void
00309         )
00310     {
00311         double freq;
00312 
00313         TLevel(API);
00314         TEnter();
00315 
00316         freq = 1.0/GetPeriod();
00317 
00318         TExitMsg(("=%f", freq));
00319         return freq;
00320     }   //GetLoopsPerSec
00321 
00335     void
00336     StartCompetition(
00337         void
00338         )
00339     {
00340         TLevel(API);
00341         TEnter();
00342 
00343         UINT32 timeAutonomousPeriod = 0;
00344         UINT32 timeTeleOpPeriod = 0;
00345         UINT32 cntLoops = 0;
00346         UINT32 timeBegin = 0;
00347         UINT32 timeUsed = 0;
00348         //
00349         // One-time robot initialization.
00350         //
00351         RobotInit();
00352         //
00353         // Loop forever, calling the appropriate mode-dependent functions.
00354         //
00355         while (true)
00356         {
00357             if (IsDisabled())
00358             {
00359                 // Call DisabledInit() if we are now just entering disabled
00360                 // mode from either a different mode or from power-on.
00361                 if(!m_DisabledInitialized)
00362                 {
00363                     DisabledInit();
00364                     m_DisabledInitialized = true;
00365                     // Reset the initialization flags for the other modes.
00366                     m_AutonomousInitialized = false;
00367                     m_TeleopInitialized = false;
00368                     TInfo(("Average Autonomous period execution time: %d ms",
00369                            timeAutonomousPeriod/cntLoops));
00370                     TInfo(("Average TeleOp period execution time: %d ms",
00371                            timeTeleOpPeriod/cntLoops));
00372                     TInfo(("Number of loops executed: %d", cntLoops));
00373                     TInfo(("Period time: %f s", GetPeriod()));
00374                     timeAutonomousPeriod = 0;
00375                     timeTeleOpPeriod = 0;
00376                     cntLoops = 0;
00377                 }
00378 
00379                 if (NextPeriodReady())
00380                 {
00381                     DisabledPeriodic();
00382                 }
00383 
00384                 DisabledContinuous();
00385             }
00386             else if (IsAutonomous())
00387             {
00388                 // Call AutonomousInit() if we are now just entering
00389                 // autonomous mode from either a different mode or from
00390                 // power-on.
00391                 if(!m_AutonomousInitialized)
00392                 {
00393                     AutonomousInit();
00394                     m_AutonomousInitialized = true;
00395                     // Reset the initialization flags for the other modes.
00396                     m_DisabledInitialized = false;
00397                     m_TeleopInitialized = false;
00398                 }
00399 
00400                 if (NextPeriodReady())
00401                 {
00402                     timeBegin = GetMsecTime();
00403                     AutonomousPeriodic();
00404                     timeUsed = GetMsecTime() - timeBegin;
00405                     timeAutonomousPeriod += timeUsed;
00406                     cntLoops++;
00407                     if ((float)timeUsed/1000.0 > GetPeriod()*0.9)
00408                     {
00409                         //
00410                         // Execution time exceeds 90% of the period.
00411                         //
00412                         TWarn(("Autonomous period executed too long (%d ms)",
00413                                timeUsed));
00414                     }
00415                 }
00416 
00417                 AutonomousContinuous();
00418             }
00419             else
00420             {
00421                 // Call TeleopInit() if we are now just entering teleop
00422                 // mode from either a different mode or from power-on.
00423                 if(!m_TeleopInitialized)
00424                 {
00425                     TeleopInit();
00426                     m_TeleopInitialized = true;
00427                     // Reset the initialization flags for the other modes.
00428                     m_DisabledInitialized = false;
00429                     m_AutonomousInitialized = false;
00430                 }
00431 
00432                 if (NextPeriodReady())
00433                 {
00434                     timeBegin = GetMsecTime();
00435                     TeleopPeriodic();
00436                     timeUsed = GetMsecTime() - timeBegin;
00437                     timeTeleOpPeriod += timeUsed;
00438                     cntLoops++;
00439                     if ((float)timeUsed/1000.0 > GetPeriod()*0.9)
00440                     {
00441                         //
00442                         // Execution time exceeds 90% of the period.
00443                         //
00444                         TWarn(("TeleOp period executed too long (%d ms)",
00445                                timeUsed));
00446                     }
00447                 }
00448 
00449                 TeleopContinuous();
00450             }
00451         }
00452 
00453         TExit();
00454     }   //StartCompetition
00455 
00456 protected:
00460     TrcRobot(
00461         void
00462         ): m_DisabledInitialized(false),
00463            m_AutonomousInitialized(false),
00464            m_TeleopInitialized(false),
00465            m_LoopPeriod(kDefaultPeriod),
00466            m_periodPacket(0)
00467     {
00468         TLevel(INIT);
00469         TEnter();
00470         TExit();
00471     }   //TrcRobot
00472 
00476     ~TrcRobot(
00477         void
00478         )
00479     {
00480         TLevel(INIT);
00481         TEnter();
00482         TExit();
00483     }   //~TrcRobot
00484 };  //class TrcRobot
00485 
00486 #endif  //ifndef _TRCROBOT_H
 All Classes Files Functions Variables Defines