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

C:/Users/Michael/Ftc/2011/code/trclib/lnfollow.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 _LNFOLLOW_H
00016 #define _LNFOLLOW_H
00017 
00018 #pragma systemFile
00019 
00020 #ifdef MOD_ID
00021     #undef MOD_ID
00022 #endif
00023 #define MOD_ID                  MOD_LNFOLLOW
00024 
00025 //
00026 // Constants.
00027 //
00028 #ifndef MAX_LIGHT_SENSORS
00029     #define MAX_LIGHT_SENSORS   3
00030 #endif
00031 
00032 #define LNF_STARTED             0x0100
00033 #define LNF_CALIBRATING         0x0200
00034 #define LnFollowStarted(l)      (l.lnfollowFlags & LNF_STARTED)
00035 #define LnFollowCalibrating(l)  (l.lnfollowFlags & LNF_CALIBRATING)
00036 
00037 //
00038 // Type definitions.
00039 //
00040 typedef struct
00041 {
00042 //    DRIVE   &drive;
00043 //    PIDCTRL &pidCtrl;
00044     int      drive;
00045     int      pidCtrl;
00046     int      numLightSensors;
00047     int      driveKp;
00048     SENSOR   LightSensors[MAX_LIGHT_SENSORS];
00049     int      valueMap[1 << MAX_LIGHT_SENSORS];
00050     int      lnfollowFlags;
00051     int      maxSensorValue;
00052     int      mapIndex;
00053     int      sensorValue;
00054 } LNFOLLOW;
00055 
00065 void
00066 LnFollowInit(
00067     __out LNFOLLOW &lnfollow,
00068 //    __in  DRIVE &drive,
00069 //    __in  PIDCTRL &pidCtrl,
00070     __in  int drive,
00071     __in  int pidCtrl,
00072     __in  int numLightSensors,
00073     __in  int driveKp
00074     )
00075 {
00076     int i;
00077 
00078     TFuncName("LnFollowInit");
00079     TLevel(INIT);
00080     TEnter();
00081 
00082     lnfollow.drive = drive;
00083     lnfollow.pidCtrl = pidCtrl;
00084     lnfollow.numLightSensors = (numLightSensors <= MAX_LIGHT_SENSORS)?
00085                                numLightSensors: MAX_LIGHT_SENSORS;
00086     lnfollow.driveKp = driveKp;
00087     lnfollow.lnfollowFlags = 0;
00088     lnfollow.maxSensorValue = lnfollow.numLightSensors*2 - 1;
00089     lnfollow.mapIndex = 0;
00090     lnfollow.sensorValue = 0;
00091 
00092     //
00093     // Initialize the "no light" value.
00094     //
00095     lnfollow.valueMap[0] = 0;
00096 
00097     //
00098     // Initialize the rest of the array with an invalid value.
00099     //
00100     for (i = 1; i < (1 << lnfollow.numLightSensors); i++)
00101     {
00102         lnfollow.valueMap[i] = -1;
00103     }
00104 
00105     for (i = 0; i < lnfollow.numLightSensors; i++)
00106     {
00107         //
00108         // Initialize the "right on" light values.
00109         //
00110         lnfollow.valueMap[1 << i] = i*2 + 1;
00111         //
00112         // Initialize the "in between" light values.
00113         //
00114         if (i < lnfollow.numLightSensors - 1)
00115         {
00116             lnfollow.valueMap[(i << i) + (i << (i + 1))] = (i + 1)*2;
00117         }
00118     }
00119 
00120     TExit();
00121     return;
00122 }   //LnFollowInit
00123 
00130 void
00131 LnFollowCal(
00132     __inout LNFOLLOW &lnfollow,
00133     __in    bool fStart
00134     )
00135 {
00136     TFuncName("LnFollowCal");
00137     TLevel(API);
00138     TEnterMsg(("fStart=%d", (byte)fStart));
00139 
00140     if (fStart)
00141     {
00142         lnfollow.lnfollowFlags |= LNF_CALIBRATING;
00143     }
00144     else
00145     {
00146         lnfollow.lnfollowFlags &= ~LNF_CALIBRATING;
00147     }
00148 
00149     for (int i = 0; i < lnfollow.numLightSensors; i++)
00150     {
00151         SensorCal(lnfollow.LightSensors[i], fStart);
00152     }
00153 
00154     TExit();
00155     return;
00156 }   //LnFollowCal
00157 
00164 void
00165 LnFollowStart(
00166     __inout LNFOLLOW &lnfollow,
00167     __in    bool fStart
00168     )
00169 {
00170     TFuncName("LnFollowStart");
00171     TLevel(API);
00172     TEnterMsg(("fStart=%d", (byte)fStart));
00173 
00174     if (fStart)
00175     {
00176         if (!LnFollowStarted(lnfollow))
00177         {
00178             lnfollow.lnfollowFlags |= LNF_STARTED;
00179             PIDCtrlSetTarget(g_PIDCtrl[lnfollow.pidCtrl],
00180                              (lnfollow.maxSensorValue + 1)/2.0, -1.0);
00181         }
00182     }
00183     else if (LnFollowStarted(lnfollow))
00184     {
00185         lnfollow.lnfollowFlags &= ~LNF_STARTED;
00186         DriveReset(g_Drive[lnfollow.drive]);
00187         PIDCtrlReset(g_PIDCtrl[lnfollow.pidCtrl]);
00188     }
00189 
00190     TExit();
00191     return;
00192 }   //LnFollowStart
00193 
00199 void
00200 LnFollowTask(
00201     __inout LNFOLLOW &lnfollow
00202     )
00203 {
00204     TFuncName("LnFollowTask");
00205     TLevel(TASK);
00206     TEnter();
00207 
00208     if (LnFollowStarted(lnfollow))
00209     {
00210         lnfollow.mapIndex = 0;
00211         for (int i = 0; i < lnfollow.numLightSensors; i++)
00212         {
00213             lnfollow.mapIndex <<= 1;
00214             if (lnfollow.LightSensors[i].sensorZone == SENSORZONE_HI)
00215             {
00216                 lnfollow.mapIndex++;
00217             }
00218         }
00219         lnfollow.sensorValue = lnfollow.valueMap[lnfollow.mapIndex];
00220 
00221         //
00222         // Performing PID control on turn.
00223         //
00224         nxtDisplayTextLine(2, "idx=%d,value=%d",
00225                            lnfollow.mapIndex, lnfollow.sensorValue);
00226 //        lnfollow.turnPower = PIDCtrlOutput(lnfollow.pidCtrl,
00227 //                                           lnfollow.weightedSensorValue);
00228         int turnPower = PIDCtrlOutput(g_PIDCtrl[lnfollow.pidCtrl]);
00229         //
00230         // Calculate the drive power: Drive power should be inversely
00231         // proportaional to the turn power. So the faster the robot turns,
00232         // the slower we will drive the robot.
00233         //
00234         int drivePower = (MOTOR_MAX_VALUE - abs(turnPower))*lnfollow.driveKp;
00235 //        DriveArcade(lnfollow.drive,
00236         DriveArcade(g_Drive[lnfollow.drive], drivePower, turnPower);
00237     }
00238 
00239     TExit();
00240     return;
00241 }   //LnFollowTask
00242 
00243 #endif  //ifndef _LNFOLLOW_H
 All Data Structures Files Functions Variables Defines