robotc

college code of robotc
subsumptionLevel1.c

(3300B)
      1 #pragma config(Sensor, S2, IRSeeker2, sensorHiTechnicIRSeeker1200)
      2 #pragma config(Sensor, S3, sonarSensor, sensorSONAR)
      3 #pragma config(Sensor, S4, analogSensor, sensorAnalogActive)
      4 //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
      5 
      6 int NEAR_SONAR = 25;
      7 int NEAR_ANALOG = 1020;
      8 
      9 /* for vector objects */
     10 struct {
     11 float dir;
     12 float mag;
     13 } VECTOR;
     14 
     15 /* Avoids obstacles. */
     16 VECTOR avoid(VECTOR& force);
     17 
     18 /* Checks if the EOPD sensor detects an obstacle. */
     19 bool obstacleNearAnalog();
     20 
     21 /* Checks if the SONAR sensor detects an obstacle. */
     22 bool obstacleNearSonar();
     23 
     24 /* Moves randomly to any direction. */
     25 void wander(VECTOR& force);
     26 
     27 /* converts raw sensor reading into force vector */
     28 void feelForce(int percept, VECTOR& force);
     29 
     30 /* converts raw sensor percept into boolean */
     31 bool collide(int percept);
     32 
     33 /* converts a force vector into a heading vector */
     34 void runaway(VECTOR const& force, VECTOR& heading);
     35 
     36 /* directs actuators to move forward at power proportional to mag */
     37 void forward(bool halt, float mag);
     38 
     39 /* directs actuators to turn by dir degrees */
     40 void turn(float dir);
     41 
     42 /* robot control loop */
     43 task main() {
     44   VECTOR force;
     45   VECTOR heading;
     46 
     47   while (true) {
     48     // print diagnostic text here
     49     float percept = SensorValue[IRSeeker2];
     50     bool halt = collide(percept);
     51     avoid(force);
     52     feelForce(percept, force);
     53     runaway(force, heading);
     54 
     55     // print diagnostic heading dir and mag here
     56     turn(heading.dir);
     57     forward(halt, heading.mag);
     58   }
     59 }
     60 
     61 /* Avoids obstacles. */
     62 void avoid(VECTOR& force) {
     63   float sonar = obstacleNearSonar();
     64   float analog = obstacleNearAnalog();
     65  
     66   if(sonar && analog) {
     67     force.mag = 1.0;
     68     force.dir = 180;
     69   } else if(sonar) {
     70     force.mag = 1.0;
     71     force.dir = 135;
     72   } else if(analog) {
     73     force.mag = 1.0;
     74     force.dir = -135;
     75   } 
     76 }
     77 
     78 /* Checks if the EOPD sensor detects an obstacle. */
     79 bool obstacleNearAnalog() {
     80   return (SensorValue[analogSensor] < NEAR_ANALOG) ? true : false;
     81 }
     82 
     83 /* Checks if the SONAR sensor detects an obstacle. */
     84 bool obstacleNearSonar() {
     85   return (SensorValue[sonarSensor] < NEAR_SONAR) ? true : false;
     86 }
     87 
     88 /*
     89  * Is it facing the IR?
     90  */
     91 bool collide(int percept){
     92   return percept != 0;
     93 }
     94 
     95 /*
     96  * Detect at what degree the IR is.
     97  */
     98 void feelForce(int percept, VECTOR& force){
     99   force.mag = (percept == 0 ? 100 : 0);
    100 
    101   if(irSensor == 0)
    102     wander(force);
    103   else
    104     force.dir = (5 - irSensor) * 22; //22 degrees
    105 }
    106 
    107 /* Moves randomly to any direction. */
    108 void wander(VECTOR& force) {
    109   force.dir = (random(1) == 1) ? 30 : -30;
    110 }
    111 
    112 /*
    113  * Set the heading vector as force vector.
    114  */
    115 void runaway(VECTOR const& force, VECTOR& heading){
    116   heading.mag = force.mag;
    117   heading.dir = force.dir;
    118 }
    119 
    120 /*
    121  * Move forward if not in halt.
    122  */
    123 void forward(bool halt, float mag) {
    124   if (halt) {
    125     motor[motorA] = 0;
    126     motor[motorC] = 0;
    127   } else {
    128     motor[motorA] = mag;
    129     motor[motorC] = mag;
    130   }
    131 }
    132 
    133 /*
    134 * dir in degrees
    135 * 348 rotation = 180 degrees
    136 */
    137 void turn(float dir){
    138   motor[motorA] = 0;
    139   motor[motorC] = 0;
    140   float rot = (dir * 348) / 180;
    141   if(rot < 0){
    142     while(nMotorEncoder[motorA] < -(rot)){
    143       motor[motorA] = 50;
    144       motor[motorC] = -50;
    145     }
    146   } else {
    147     while(nMotorEncoder[motorA] < rot){
    148       motor[motorA] = -50;
    149       motor[motorC] = 50;
    150     }
    151   }
    152 }