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

C:/Users/Michael/Ftc/2011/code/trclib/piddrive.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 _PIDDRIVE_H
00017 #define _PIDDRIVE_H
00018 
00019 #pragma systemFile
00020 
00021 #ifdef MOD_ID
00022     #undef MOD_ID
00023 #endif
00024 #define MOD_ID                  MOD_PIDDRIVE
00025 
00026 //
00027 // Constants.
00028 //
00029 #define PIDDRIVEF_PIDMODE_ON    0x0100
00030 #define PIDDRIVEF_STOP_ONTARGET 0x0200
00031 #define PIDDRIVEF_USER_MASK     0x00ff
00032 #define PIDDRIVEF_ENABLE_EVENTS 0x0001
00033 
00034 //
00035 // Type definitions.
00036 //
00037 typedef struct
00038 {
00039 //    DRIVE   &drive;
00040 //    PIDCTRL &drivePIDCtrl;
00041 //    PIDCTRL &turnPIDCtrl;
00042     int      drive;
00043     int      drivePIDCtrl;
00044     int      turnPIDCtrl;
00045     int      pidDriveFlags;
00046 } PIDDRIVE;
00047 
00048 //
00049 // Import function prototypes.
00050 //
00051 void
00052 PIDDriveEvent(
00053     __in PIDDRIVE &pidDrive
00054     );
00055 
00061 void
00062 PIDDriveReset(
00063     __out PIDDRIVE &pidDrive
00064     )
00065 {
00066     TFuncName("PIDDriveReset");
00067     TLevel(API);
00068     TEnter();
00069 
00070     pidDrive.pidDriveFlags &= ~PIDDRIVEF_PIDMODE_ON;
00071 //    DriveReset(pidDrive.drive);
00072 //    PIDCtrlReset(pidDrive.drivePIDCtrl);
00073 //    PIDCtrlReset(pidDrive.turnPIDCtrl);
00074     DriveReset(g_Drive[pidDrive.drive]);
00075     PIDCtrlReset(g_PIDCtrl[pidDrive.drivePIDCtrl]);
00076     PIDCtrlReset(g_PIDCtrl[pidDrive.turnPIDCtrl]);
00077     DriveStallProtect(g_Drive[pidDrive.drive], false);
00078 
00079     TExit();
00080     return;
00081 }   //PIDDriveReset
00082 
00092 void
00093 PIDDriveInit(
00094     __out PIDDRIVE &pidDrive,
00095 //    __in  DRIVE &drive,
00096 //    __in  PIDCTRL &drivePIDCtrl,
00097 //    __in  PIDCTRL &turnPIDCtrl,
00098     __in  int drive,
00099     __in  int drivePIDCtrl,
00100     __in  int turnPIDCtrl,
00101     __in  int pidDriveFlags
00102     )
00103 {
00104     TFuncName("PIDDriveInit");
00105     TLevel(INIT);
00106     TEnter();
00107 
00108     pidDrive.drive = drive;
00109     pidDrive.drivePIDCtrl = drivePIDCtrl;
00110     pidDrive.turnPIDCtrl = turnPIDCtrl;
00111     pidDrive.pidDriveFlags = pidDriveFlags & PIDDRIVEF_USER_MASK;
00112 
00113     TExit();
00114     return;
00115 }   //PIDDriveInit
00116 
00131 void
00132 PIDDriveSetTarget(
00133     __out PIDDRIVE &pidDrive,
00134     __in  float distSetPoint,
00135     __in  float distTolerance,
00136     __in  float angleSetPoint,
00137     __in  float angleTolerance,
00138     __in  bool fStopOnTarget
00139     )
00140 {
00141     TFuncName("PIDDriveSetTarget");
00142     TLevel(API);
00143     TEnterMsg(("D=%5.1f,A=%5.1f", distSetPoint, angleSetPoint));
00144 
00145 //    PIDCtrlSetTarget(pidDrive.drivePIDCtrl, distSetPoint);
00146 //    PIDCtrlSetTarget(pidDrive.turnPIDCtrl, angleSetPoint);
00147     PIDCtrlSetTarget(g_PIDCtrl[pidDrive.drivePIDCtrl],
00148                      distSetPoint,
00149                      distTolerance);
00150     PIDCtrlSetTarget(g_PIDCtrl[pidDrive.turnPIDCtrl],
00151                      angleSetPoint,
00152                      angleTolerance);
00153     if (fStopOnTarget)
00154     {
00155         pidDrive.pidDriveFlags |= PIDDRIVEF_STOP_ONTARGET;
00156     }
00157     else
00158     {
00159         pidDrive.pidDriveFlags &= ~PIDDRIVEF_STOP_ONTARGET;
00160     }
00161     pidDrive.pidDriveFlags |= PIDDRIVEF_PIDMODE_ON;
00162     DriveStallProtect(g_Drive[pidDrive.drive], true);
00163 
00164     TExit();
00165     return;
00166 }   //PIDDriveSetTarget
00167 
00173 void
00174 PIDDriveTask(
00175     __inout PIDDRIVE &pidDrive
00176     )
00177 {
00178     int output;
00179 
00180     TFuncName("PIDDriveTask");
00181     TLevel(TASK);
00182     TEnter();
00183 
00184     if (pidDrive.pidDriveFlags & PIDDRIVEF_PIDMODE_ON)
00185     {
00186 //        int drivePower = (int)PIDCtrlOutput(pidDrive.drivePIDCtrl);
00187 //        int turnPower = (int)PIDCtrlOutput(pidDrive.turnPIDCtrl);
00188         int drivePower = (int)PIDCtrlOutput(g_PIDCtrl[pidDrive.drivePIDCtrl]);
00189         int turnPower = (int)PIDCtrlOutput(g_PIDCtrl[pidDrive.turnPIDCtrl]);
00190 //        if (PIDCtrlIsOnTarget(pidDrive.drivePIDCtrl,
00191 //                              pidDrive.driveTolerance) &&
00192 //            PIDCtrlIsOnTarget(pidDrive.turnPIDCtrl,
00193 //                              pidDrive.turnTolerance))
00194         if (PIDCtrlIsOnTarget(g_PIDCtrl[pidDrive.drivePIDCtrl],
00195                               pidDrive.driveTolerance) &&
00196             PIDCtrlIsOnTarget(g_PIDCtrl[pidDrive.turnPIDCtrl],
00197                               pidDrive.turnTolerance))
00198         {
00199             if (pidDrive.pidDriveFlags & PIDDRIVEF_STOP_ONTARGET)
00200             {
00201                 PIDDriveReset(pidDrive);
00202                 if (pidDrive.pidDriveFlags & PIDDRIVEF_ENABLE_EVENTS)
00203                 {
00204                     PIDDriveEvent(pidDrive);
00205 //                    nxtDisplayTextLine(
00206 //                        1, "DE=%3.1f,TE=%3.1f",
00207 //                        g_PIDCtrl[pidDrive.drivePIDCtrl].prevError,
00208 //                        g_PIDCtrl[pidDrive.turnPIDCtrl].prevError);
00209                 }
00210             }
00211             else
00212             {
00213                 DriveStop(g_Drive[pidDrive.drive]);
00214             }
00215         }
00216         else
00217         {
00218             DriveArcade(g_Drive[pidDrive.drive], drivePower, turnPower);
00219 //            nxtDisplayTextLine(1, "D=%d,T=%d", drivePower, turnPower);
00220         }
00221     }
00222 
00223     TExit();
00224     return;
00225 }   //PIDDriveTask
00226 
00227 #endif  //ifndef _PIDDRIVE_H
 All Data Structures Files Functions Variables Defines