robotc

college code of robotc
potential.c

(3578B)
      1 #pragma config(Sensor, S1, touchSensor, sensorTouch)
      2 #pragma config(Sensor, S2, irSensor, sensorHiTechnicIRSeeker1200)
      3 #pragma config(Sensor, S3, sonarSensor, sensorSONAR)
      4 #pragma config(Sensor, S4, analogSensor, sensorAnalogActive)
      5 
      6 // 10 inches or less is considered near
      7 int NEAR_SONAR = 25;
      8 int NEAR_ANALOG = 1020;
      9 int SPEED = 50;
     10 
     11 typedef struct {
     12   float dir;
     13   float mag;
     14 } VECTOR;
     15 
     16 void vectorSum(VECTOR const& a, VECTOR const& b, VECTOR& sum)
     17 {
     18   float atheta = PI * a.dir / 180;
     19   float btheta = PI * b.dir / 180;
     20   float ax = a.mag * cos(atheta);
     21   float ay = a.mag * sin(atheta);
     22   float bx = b.mag * cos(btheta);
     23   float by = b.mag * sin(btheta);
     24   float rx = ax + bx;
     25   float ry = ay + by;
     26   float r = sqrt(rx * rx + ry * ry);
     27 
     28   //nxtDisplayCenteredTextLine(5, "%f %f", ax, bx);
     29 
     30   sum.mag = r;
     31 
     32   if (rx == 0)
     33     sum.dir = (ry > 0 ? 90 : (ry < 0 ? -90 : 0));
     34   else
     35   {
     36     sum.dir = 180 * atan(ry / rx) / PI; // arctan approximation
     37     if (rx < 0) {
     38       if (ry < 0)
     39         sum.dir = sum.dir - 180;
     40       else
     41         sum.dir = 180 + sum.dir;
     42     }
     43   }
     44   //nxtDisplayCenteredTextLine(6, "%f %f", sum.mag, sum.dir);
     45 }
     46 
     47 /* Checks if the EOPD sensor detects an obstacle. */
     48 bool obstacleNearAnalog() {
     49   return (SensorValue[analogSensor] < NEAR_ANALOG) ? true : false;
     50 }
     51 
     52 /* Checks if the SONAR sensor detects an obstacle. */
     53 bool obstacleNearSonar() {
     54   return (SensorValue[sonarSensor] < NEAR_SONAR) ? true : false;
     55 }
     56 
     57 /*
     58  * Creates a Vector that runs away from obstacles
     59  * detected by SONAR and EOPD sensor.
     60  *
     61  * Assumes the SONAR is facing 45 degrees left of the robot.
     62  * Assumes the EOPD is facing 45 degrees right of the robot.
     63  */
     64 void runaway(VECTOR& v) {
     65   bool sonar = obstacleNearSonar();
     66   bool analog = obstacleNearAnalog();
     67 
     68   VECTOR analogVector;
     69   analogVector.mag = (analog) ? 1.0 : 0;
     70   analogVector.dir = (analog) ? 135 : 0;
     71 
     72   VECTOR sonarVector;
     73   sonarVector.mag = (sonar) ? 1.0 : 0;
     74   sonarVector.dir = (sonar) ? -135 : 0;
     75 
     76   vectorSum(analogVector, sonarVector, v);
     77 }
     78 
     79 /*
     80  * Directs Vector v to the IR signal.
     81  * Moves left or right by 30 degrees
     82  * if no IR signal is detected.
     83  */
     84 void move2Goal(VECTOR& v) {
     85   int irSensor = SensorValue[S2];
     86   v.mag = 1.0;
     87 
     88   if(irSensor == 0)
     89     v.dir = (random(1) == 1) ? 30 : -30;
     90   else
     91     v.dir = (5 - irSensor) * 22; //22 degrees
     92 }
     93 
     94 /*
     95  * Turns the robot by a rotation value.
     96  * A rotation value of 348 is equivalent
     97  * to 180 degree turn.
     98  */
     99 void turn(float rot)
    100 {
    101   float dir = (rot * 348) / 180;
    102   float speed = abs(dir / 5.8);
    103   if (dir > 0)
    104   {
    105     motor[motorA] = -speed;
    106     motor[motorC] = speed;
    107     wait1Msec(500);
    108   }
    109   else if (dir < 0)
    110   {
    111     motor[motorA] = speed;
    112     motor[motorC] = -speed;
    113     wait1Msec(500);
    114   }
    115   motor[motorA] = 0;   //power level of 100.
    116   motor[motorC] = 0;
    117 }
    118 
    119 /*
    120  * Moves the robot forward by its speed and magnitude.
    121  */
    122 void move(float mag) {
    123   motor[motorA] = mag * SPEED;   //Motor A & C is at
    124   motor[motorC] = mag * SPEED;   //power level of 100.
    125   wait1Msec(500);
    126 }
    127 
    128 task main() {
    129 
    130   VECTOR goalVector, repulseVector, attractVector;
    131 
    132   nxtDisplayCenteredTextLine(0, "Running...");
    133   while(true) {
    134     move2Goal(goalVector);
    135     nxtDisplayCenteredTextLine(1, "%f %f", goalVector.dir, goalVector.mag);
    136 
    137     runaway(repulseVector);
    138     nxtDisplayCenteredTextLine(2, "%f %f", repulseVector.dir, repulseVector.mag);
    139 
    140     vectorSum(goalVector, repulseVector, attractVector);
    141     nxtDisplayCenteredTextLine(3, "%f %f", attractVector.dir, attractVector.mag);
    142 
    143     // now move robot
    144     turn(attractVector.dir);
    145     move(attractVector.mag);
    146   }
    147 
    148 }