ftclib/radar.h
author Trc492/3543
Fri Jan 11 23:43:11 2013 -0800 (2013-01-11)
changeset 109 831df22ca31d
parent 17 bd292878e64d
permissions -rw-r--r--
Prep for state championship
     1 #if 0
     2 /// Copyright (c) Titan Robotics Club. All rights reserved.
     3 ///
     4 /// <module name="radar.h" />
     5 ///
     6 /// <summary>
     7 ///     This module contains the library functions for the sonar sensor
     8 ///     on a rotating platform.
     9 /// </summary>
    10 ///
    11 /// <remarks>
    12 ///     Environment: RobotC for Lego Mindstorms NXT.
    13 /// </remarks>
    14 #endif
    15 
    16 #ifndef _RADAR_H
    17 #define _RADAR_H
    18 
    19 #pragma systemFile
    20 
    21 #ifdef MOD_ID
    22     #undef MOD_ID
    23 #endif
    24 #define MOD_ID                  MOD_RADAR
    25 
    26 //
    27 // Constants.
    28 //
    29 #define RADARO_SERVO_MOTOR      0x0001
    30 #define RADARO_INVERSE          0x0002  //for encoder only
    31 
    32 #define RADARF_ENABLED          0x0001
    33 #define RADARF_DIR_LOW          0x0002
    34 
    35 #ifndef RADAR_CLICKS_PER_DEGREE
    36     #define RADAR_CLICKS_PER_DEGREE     1.0
    37 #endif
    38 #ifndef RADAR_SAMPLING_DELAY
    39     #define RADAR_SAMPLING_DELAY        100
    40 #endif
    41 #ifndef MAX_SAMPLE_POINTS
    42     #define MAX_SAMPLE_POINTS           4
    43 #endif
    44 
    45 //
    46 // Macros.
    47 //
    48 
    49 /**
    50  *  This macro checks if the radar is enabled.
    51  *
    52  *  @param r Points to the RADAR structure.
    53  *
    54  *  @return Returns true if the radar is enabled.
    55  */
    56 #define RadarIsEnabled(r)               ((r).flags & RADARF_ENABLED)
    57 
    58 //
    59 // Type definitions.
    60 //
    61 typedef struct
    62 {
    63     tSensors    sonarID;
    64     tMotor      motorID;
    65     float       posPerDegree;
    66     float       Kp;             //for encoder only
    67     int         motorPower;     //motor power limit
    68     long        samplingDelay;
    69     int         options;
    70     int         flags;
    71     int         numSamples;
    72     float       sampleAngles[MAX_SAMPLE_POINTS];
    73     int         samplePoints[MAX_SAMPLE_POINTS];    //for encoder only
    74     int         sampleData[MAX_SAMPLE_POINTS];
    75     int         nextSample;
    76     long        samplingTime;
    77 } RADAR;
    78 
    79 /**
    80  *  This function initializes the radar subsystem.
    81  *
    82  *  @param radar Points to the RADAR structure.
    83  *  @param sonarID Specifies the ID of the sonar sensor.
    84  *  @param motorID Specifies the ID of the motor.
    85  *  @param posPerDegree Specifes the encoder clicks per degree or
    86  *         position per degree for servo motor.
    87  *  @param Kp Specifies the proportional PID constant for the rotation.
    88  *  @param motorPower Specifies the motor power limit (must be positive).
    89  *  @param samplingDelay Specifies the sampling delay time in msec.
    90  *  @param options Optionally specifies the radar options:
    91  *         RADARO_SERVO_MOTOR - Servo motor is used.
    92  *         RADARO_INVERSE - Motor encoder is inverse.
    93  */
    94 void
    95 RadarInit(
    96     RADAR &radar,
    97     tSensors sonarID,
    98     tMotor motorID,
    99     float posPerDegree,
   100     float Kp,
   101     int motorPower,
   102     long samplingDelay = RADAR_SAMPLING_DELAY,
   103     int options = 0
   104     )
   105 {
   106     TFuncName("RadarInit");
   107     TLevel(INIT);
   108     TEnter();
   109 
   110     radar.sonarID = sonarID;
   111     radar.motorID = motorID;
   112     radar.options = options;
   113     radar.flags = 0;
   114     radar.posPerDegree = posPerDegree;
   115     if (!(options & RADARO_SERVO_MOTOR))
   116     {
   117         nMotorEncoder[motorID] = 0;
   118         radar.Kp = Kp;
   119     }
   120     else
   121     {
   122         //
   123         // INVERSE flag is not applicable for servo motor.
   124         //
   125         radar.options &= ~RADARO_INVERSE;
   126     }
   127     radar.motorPower = abs(motorPower);
   128     radar.samplingDelay = samplingDelay;
   129     radar.numSamples = 0;
   130     radar.nextSample = 0;
   131     radar.samplingTime = 0;
   132 
   133     TExit();
   134     return;
   135 }   //RadarInit
   136 
   137 /**
   138  *  This function adds a sampling angle.
   139  *
   140  *  @param radar Points to the RADAR structure.
   141  *  @param angle Specifies the sample angling in degrees.
   142  *
   143  *  @return Returns true if successful, false otherwise.
   144  */
   145 bool
   146 RadarAddSample(
   147     RADAR &radar,
   148     float angle
   149     )
   150 {
   151     bool fSuccess = false;
   152     int i, j;
   153 
   154     TFuncName("RadarAddSample");
   155     TLevel(API);
   156     TEnter();
   157 
   158     if (radar.numSamples < MAX_SAMPLE_POINTS)
   159     {
   160         if (radar.options & RADARO_SERVO_MOTOR)
   161         {
   162             //
   163             // The sample angles must be sorted, so let's do an insertion
   164             // sort here.
   165             //
   166             for (i = 0; i < radar.numSamples; i++)
   167             {
   168                 if (angle < radar.sampleAngles[i])
   169                 {
   170                     for (j = radar.numSamples - 1; j >= i; j--)
   171                     {
   172                         radar.sampleAngles[j + 1] = radar.sampleAngles[j];
   173                     }
   174                     fSuccess = true;
   175                     break;
   176                 }
   177                 else if (angle == radar.sampleAngles[i])
   178                 {
   179                     //
   180                     // If sample angle already exists, don't add another one.
   181                     //
   182                     break;
   183                 }
   184             }
   185 
   186             if (fSuccess || (i == radar.numSamples))
   187             {
   188                 radar.sampleAngles[i] = angle;
   189                 radar.numSamples++;
   190                 fSuccess = true;
   191             }
   192         }
   193         else
   194         {
   195             int point = (radar.options & RADARO_INVERSE)?
   196                             (int)(-angle*radar.posPerDegree):
   197                             (int)(angle*radar.posPerDegree);
   198             //
   199             // The sample points must be sorted, so let's do an insertion sort
   200             // here.
   201             //
   202             for (i = 0; i < radar.numSamples; i++)
   203             {
   204                 if (point < radar.samplePoints[i])
   205                 {
   206                     for (j = radar.numSamples - 1; j >= i; j--)
   207                     {
   208                         radar.sampleAngles[j + 1] = radar.sampleAngles[j];
   209                         radar.samplePoints[j + 1] = radar.samplePoints[j];
   210                     }
   211                     fSuccess = true;
   212                     break;
   213                 }
   214                 else if (point == radar.samplePoints[i])
   215                 {
   216                     //
   217                     // If sample point already exists, don't add another one.
   218                     //
   219                     break;
   220                 }
   221             }
   222 
   223             if (fSuccess || (i == radar.numSamples))
   224             {
   225                 radar.sampleAngles[i] = angle;
   226                 radar.samplePoints[i] = point;
   227                 radar.numSamples++;
   228                 fSuccess = true;
   229             }
   230         }
   231     }
   232 
   233     TExitMsg(("=%d", fSuccess));
   234     return fSuccess;
   235 }   //RadarAddSample
   236 
   237 /**
   238  *  This function gets the sample data for the angle sampling point.
   239  *
   240  *  @param radar Points to the RADAR structure.
   241  *  @param angle Specifies the sampling angle to get its data.
   242  *
   243  *  @return If success, it returns the sonar reading of the given angle.
   244  *          If failure, it returns a negative value.
   245  */
   246 float
   247 RadarGetData(
   248     RADAR &radar,
   249     float angle
   250     )
   251 {
   252     float data = -1.0;
   253 
   254     TFuncName("RadarGetData");
   255     TLevel(API);
   256     TEnterMsg(("Angle=%5.1f", angle));
   257 
   258     for (int i = 0; i < radar.numSamples; i++)
   259     {
   260         if (angle == radar.sampleAngles[i])
   261         {
   262             data = radar.sampleData[i]*INCHES_PER_CM;
   263             break;
   264         }
   265     }
   266 
   267     TExitMsg(("=%5.1f", data));
   268     return data;
   269 }   //RadarGetData
   270 
   271 /**
   272  *  This function enables or disables the radar subsystem.
   273  *
   274  *  @param radar Points to the RADAR structure.
   275  *  @param fEnable If true, enable the radar subsystem, disable otherwise.
   276  */
   277 void
   278 RadarSetState(
   279     RADAR &radar,
   280     bool fEnable
   281     )
   282 {
   283     TFuncName("RadarSetState");
   284     TLevel(API);
   285     TEnterMsg(("fEnable=%d", fEnable));
   286 
   287     if (fEnable)
   288     {
   289         if (radar.options & RADARO_SERVO_MOTOR)
   290         {
   291             radar.samplingTime = nPgmTime + radar.samplingDelay;
   292         }
   293         else
   294         {
   295             int currPos = nMotorEncoder[radar.motorID];
   296 
   297             radar.nextSample = radar.numSamples - 1;
   298             for (int i = 0; i < radar.numSamples; i++)
   299             {
   300                 if (currPos < radar.samplePoints[i])
   301                 {
   302                     radar.nextSample = i;
   303                     break;
   304                 }
   305             }
   306 
   307             if (currPos < radar.samplePoints[radar.nextSample])
   308             {
   309                 radar.flags &= ~RADARF_DIR_LOW;
   310             }
   311             else
   312             {
   313                 radar.flags |= RADARF_DIR_LOW;
   314             }
   315 
   316             radar.samplingTime = 0;
   317         }
   318 
   319         radar.flags |= RADARF_ENABLED;
   320     }
   321     else
   322     {
   323         if (!(radar.options & RADARO_SERVO_MOTOR))
   324         {
   325             motor[radar.motorID] = 0;
   326         }
   327         radar.flags &= ~RADARF_ENABLED;
   328     }
   329 
   330     TExit();
   331     return;
   332 }   //RadarSetEnable
   333 
   334 /**
   335  *  This function records the sonar reading and calculate the next sample
   336  *  point.
   337  *
   338  *  @param radar Points to the RADAR structure.
   339  */
   340 void
   341 RadarSampleData(
   342     RADAR &radar
   343     )
   344 {
   345     TFuncName("RadarSampleData");
   346     TLevel(FUNC);
   347     TEnter();
   348 
   349     radar.sampleData[radar.nextSample] = SensorValue[radar.sonarID];
   350     if (radar.flags & RADARF_DIR_LOW)
   351     {
   352         if (radar.nextSample == 0)
   353         {
   354             //
   355             // Reached the low limit, reverse.
   356             //
   357             radar.nextSample = 1;
   358             radar.flags &= ~RADARF_DIR_LOW;
   359         }
   360         else
   361         {
   362             radar.nextSample--;
   363         }
   364     }
   365     else
   366     {
   367         if (radar.nextSample == radar.numSamples - 1)
   368         {
   369             //
   370             // Reached the high limit, reverse.
   371             //
   372             radar.nextSample = radar.numSamples - 2;
   373             radar.flags |= RADARF_DIR_LOW;
   374         }
   375         else
   376         {
   377             radar.nextSample++;
   378         }
   379     }
   380 
   381     TExit();
   382     return;
   383 }   //RadarSampleData
   384 
   385 /**
   386  *  This function performs the radar task where it will turn the sonar sensor
   387  *  left and right.
   388  *
   389  *  @param radar Points to the RADAR structure.
   390  */
   391 void
   392 RadarTask(
   393     RADAR &radar
   394     )
   395 {
   396     TFuncName("RadarTask");
   397     TLevel(TASK);
   398     TEnter();
   399 
   400     if (radar.flags & RADARF_ENABLED)
   401     {
   402         long currTime = nPgmTime;
   403 
   404         if (radar.options & RADARO_SERVO_MOTOR)
   405         {
   406             if (currTime >= radar.samplingTime)
   407             {
   408                 //
   409                 // The servo has reached the next sample point, take a sample,
   410                 // calculate the next sample point and program the servo to
   411                 // go to the next sample point.
   412                 //
   413                 RadarSampleData(radar);
   414                 servo[(TServoIndex)radar.motorID] =
   415                     radar.sampleAngles[radar.nextSample]*radar.posPerDegree;
   416                 radar.samplingTime = currTime + radar.samplingDelay;
   417             }
   418         }
   419         else
   420         {
   421             int currPos = nMotorEncoder[radar.motorID];
   422 
   423 
   424             if (radar.samplingTime == 0)
   425             {
   426                 //
   427                 // The motor still moving, check for target and apply PID.
   428                 //
   429                 if ((radar.flags & RADARF_DIR_LOW) &&
   430                     (currPos <= radar.samplePoints[radar.nextSample]) ||
   431                     !(radar.flags & RADARF_DIR_LOW) &&
   432                     (currPos >= radar.samplePoints[radar.nextSample]))
   433                 {
   434                     //
   435                     // We have reached the next sample point. Stop the motor,
   436                     // wait for it to settle before we take the sample.
   437                     //
   438                     motor[radar.motorID] = 0;
   439                     radar.samplingTime = currTime + radar.samplingDelay;
   440                 }
   441             }
   442             else if (currTime >= radar.samplingTime)
   443             {
   444                 //
   445                 // The motor has reached the next sample point, take a sample
   446                 // and calculate the next sample point.
   447                 //
   448                 RadarSampleData(radar);
   449                 radar.samplingTime = 0;
   450             }
   451 
   452             if (radar.samplingTime == 0)
   453             {
   454                 //
   455                 // The motor should be moving, do PID control on the motor.
   456                 //
   457                 int error = radar.samplePoints[radar.nextSample] - currPos;
   458                 motor[radar.motorID] = BOUND(error*radar.Kp,
   459                                              -radar.motorPower,
   460                                              radar.motorPower);
   461             }
   462         }
   463     }
   464 
   465     TExit();
   466     return;
   467 }   //RadarTask
   468 
   469 #endif  //ifndef _RADAR_H