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

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

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 #endif
00015 
00016 #ifndef _TRCPIDDRIVE_H
00017 #define _TRCPIDDRIVE_H
00018 
00019 #ifdef MOD_ID
00020     #undef MOD_ID
00021 #endif
00022 #define MOD_ID                  MOD_PIDDRIVE
00023 #ifdef MOD_NAME
00024     #undef MOD_NAME
00025 #endif
00026 #define MOD_NAME                "TrcPIDDrive"
00027 
00028 //
00029 // Constants.
00030 //
00031 #define PIDDRIVEF_PIDDRIVE_ON   0x00000001
00032 #define PIDDRIVEF_STOP_ONTARGET 0x00000002
00033 
00040 class PIDDriveNotify
00041 {
00042 public:
00050     virtual
00051     void
00052     NotifyPIDDrive(
00053         __in TrcPIDDrive *pidDrive
00054         ) = 0;
00055 };  //class PIDDriveNotify
00056 
00064 class TrcPIDDrive
00065 {
00066 private:
00067     RobotDrive     *m_drive;
00068     TrcPIDCtrl     *m_pidCtrlDrive;
00069     TrcPIDCtrl     *m_pidCtrlTurn;
00070     PIDDriveNotify *m_notify;
00071     UINT32          m_flags;
00072 
00073 public:
00084     void
00085     TrcPIDDrive(
00086         __in RobotDrive     *drive,
00087         __in TrcPIDCtrl     *pidCtrlDrive,
00088         __in TrcPIDCtrl     *pidCtrlTurn,
00089         __in PIDDriveNotify *notify = NULL
00090         )
00091     {
00092         TLevel(INIT);
00093         TEnterMsg(("drive=%p,pidCtrlDrive=%p,pidCtrlTurn=%p,notify=%p",
00094                    drive, pidCtrlDrive, pidCtrlTurn, notify));
00095 
00096         m_drive = drive;
00097         m_pidCtrlDrive = pidCtrlDrive;
00098         m_pidCtrlTurn = pidCtrlTurn;
00099         m_notify = notify;
00100         m_flags = 0;
00101 
00102         TExit();
00103     }   //TrcPIDDrive
00104 
00108     ~TrcPIDDrive(
00109         void
00110         )
00111     {
00112         TLevel(INIT);
00113         TEnter();
00114         TExit();
00115     }   //~TrcPIDDrive
00116 
00120     void
00121     Reset(
00122         void
00123         )
00124     {
00125         TLevel(API);
00126         TEnter();
00127 
00128         m_flags &= ~PIDDRIVEF_PIDDRIVE_ON;
00129         Drive(0.0, 0.0);
00130         m_pidCtrlDrive->Reset();
00131         m_pidCtrlTurn->Reset();
00132 //        m_drive->SetStallProtection(false);
00133 
00134         TExit();
00135         return;
00136     }   //Reset
00137 
00152     void
00153     SetTarget(
00154         __in float distSetPoint,
00155         __in float distTolerance,
00156         __in float angleSetPoint,
00157         __in float angleTolerance,
00158         __in bool  fStopOnTarget
00159         )
00160     {
00161         TLevel(API);
00162         TEnterMsg(("distSetPt=%f,distTolerance=%f,angleSetPt=%f,angleTolerance=%f,fStopOnTarget=%x",
00163                    distSetPoint, distTolerance, angleSetPoint, angleTolerance,
00164                    fStopOnTarget));
00165 
00166         m_pidCtrlDrive->SetTarget(distSetPoint, distTolerance);
00167         m_pidCtrlTurn->SetTarget(angleSetPoint, angleTolerance);
00168         if (fStopOnTarget)
00169         {
00170             m_flags |= PIDDRIVEF_STOP_ONTARGET;
00171         }
00172         else
00173         {
00174             m_flags &= ~PIDDRIVEF_STOP_ONTARGET;
00175         }
00176         m_flags |= PIDDRIVEF_PIDDRIVE_ON;
00177 //        m_drive->SetStallProtection(true);
00178 
00179         TExit();
00180         return;
00181     }   //SetTarget
00182 
00187     void
00188     PIDDriveTask(
00189         void
00190         )
00191     {
00192         TLevel(TASK);
00193         TEnter();
00194 
00195         if ((m_flags & PIDDRIVEF_PIDDRIVE_ON) &&
00196             (m_pidCtrlDrive != NULL) && (m_pidCtrlTurn != NULL))
00197         {
00198             if (m_pidCtrlDrive->OnTarget() && m_pidCtrlTurn->OnTarget())
00199             {
00200                 if (m_flags & PIDDRIVEF_STOP_ONTARGET)
00201                 {
00202                     Reset();
00203                     if (m_notify != NULL)
00204                     {
00205                         m_notify->NotifyPIDDrive(this);
00206                     }
00207                 }
00208                 else
00209                 {
00210                     m_drive->Drive(0.0, 0.0);
00211                 }
00212             }
00213             else
00214             {
00215                 m_drive->ArcadeDrive(m_pidCtrlDrive->CalcPIDOutput(),
00216                                      m_pidCtrlTurn->CalcPIDOutput());
00217             }
00218         }
00219 
00220         TExit();
00221         return;
00222     }   //PIDDriveTask
00223 };  //class TrcPIDDrive
00224 
00225 #endif  //ifndef _TRCPIDDRIVE_H
 All Classes Files Functions Variables Defines