functions.cpp

00001 // The following file implements all the functions necessary 
00002 // for the robot.
00003 // Please don't modify any of these functions!
00004 
00005 #include "mbed.h"
00006 #include "Motor.h"
00007 #include "hcsr04.h"
00008 #include "functions.h"
00009 
00010 DigitalOut myled1(LED1);
00011 DigitalOut myled2(LED2);
00012 DigitalOut myled3(LED3);
00013 DigitalOut myled4(LED4);
00014 
00015 // Two sensors are used to detect a line, line_sens1 and line_sens2
00016 // if there is a difference between the readings of these sensors 
00017 // the robot has detected a line.
00018 // Setting pins for line sensor
00019 DigitalInOut line1(p29);
00020 DigitalInOut line2(p22);
00021 
00022 // Setting pins for motor, as follows:
00023 // Example: Motor____(PWM, Forward, Reverse)
00024 Motor MotorLeft(p24, p13, p14); // pwm, fwd, rev
00025 Motor MotorRight(p23, p11, p10); // pwm, fwd, rev
00026 
00027 // Setting pins for ultrasonic sensor, as follows:
00028 // Example: usensor(Trigger, Echo)
00029 HCSR04 usensor(p25,p26);
00030 
00031 void turn_led_on(int LED) {
00032         switch(LED) {
00033             case 1:
00034                 myled1 = 1;
00035                 wait(0.1);
00036                 myled1 = 0;
00037                 break;
00038             case 2:
00039                 myled2 = 1;
00040                 wait(0.1);
00041                 myled2 = 0;
00042                 break;
00043             case 3:
00044                 myled3 = 1;
00045                 wait(0.1);
00046                 myled3 = 0;
00047                 break;
00048             case 4:
00049                 myled4 = 1;
00050                 wait(0.1);
00051                 myled4 = 0;
00052                 break;
00053         }
00054 }
00055 
00056 void turn_leds_on() {
00057         myled1 = 1;
00058         myled2 = 1;
00059         myled3 = 1;
00060         myled4 = 1;
00061         wait(0.1);
00062         myled1 = 0;
00063         myled2 = 0;
00064         myled3 = 0;
00065         myled4 = 0;
00066 }
00067 
00068 void turn_led_left() {
00069         myled1 = 1;
00070         myled2 = 1;
00071         wait(0.1);
00072         myled1 = 0;
00073         myled2 = 0;
00074 }
00075 
00076 void turn_led_right() {
00077         myled3 = 1;
00078         myled4 = 1;
00079         wait(0.1);
00080         myled3 = 0;
00081         myled4 = 0;
00082 }
00083 
00084 void flash_leds() {
00085     Timer t;
00086     t.start();
00087     while(t.read_ms() < 1000) {
00088         myled1 = 0;
00089         myled2 = 0;
00090         myled3 = 0;
00091         myled4 = 0;
00092         wait(0.15);
00093         myled1 = 1;
00094         wait(0.15);
00095         myled2 = 1;
00096         wait(0.15);
00097         myled3 = 1;
00098         wait(0.15);
00099         myled4 = 1;
00100         wait(0.15);
00101         myled1 = 0;
00102         myled2 = 0;
00103         myled3 = 0;
00104         myled4 = 0;
00105     }
00106     t.stop();
00107 }
00108 
00109 // Returns value from the QRE1113
00110 // lower numbers mean more refleacive
00111 // more than 3000 means nothing was reflected.
00112 int read_line1() {
00113     // Time to record how long input is active
00114     Timer temp_t;
00115     
00116     // Activate the line sensor and then read
00117     line1.output();
00118     line1 = 1;
00119     wait_us(15);
00120     line1.input();
00121     
00122     // Start timer
00123     temp_t.start();
00124 
00125     // Time how long the input is HIGH, but quit
00126     // after 1ms as nothing happens after that
00127     while (line1 == 1 && temp_t.read_us() < 1000);
00128     return temp_t.read_us();
00129 }
00130 
00131 int read_line2() {
00132     // Time to record how long input is active
00133     Timer temp_t;
00134     
00135     // Activate the line sensor and then read
00136     line2.output();
00137     line2 = 1;
00138     wait_us(15);
00139     line2.input();
00140     
00141     // Start timer
00142     temp_t.start();
00143 
00144     // Time how long the input is HIGH, but quit
00145     // after 1ms as nothing happens after that
00146     while (line2 == 1 && temp_t.read_us() < 1000);
00147     return temp_t.read_us();
00148 }
00149 
00150 int detect_line() {
00151     int line1val = read_line1();
00152     int line2val = read_line2();
00153     
00154     printf("LineSensor readings: %d \n", line1val-line2val);
00155     
00156     if ((line1val-line2val) > 70) {
00157         //printf("Line detected from front");
00158         return 1;
00159     } else if ((line1val-line2val) < -70) {
00160         //printf("Line detected from back");
00161         return -1;
00162     } else {
00163         //printf("Line not detected");
00164         return 0;
00165     }
00166 }
00167 
00168 void reverse(float speed, float adj) {
00169     //printf("Reverse\n");
00170     MotorLeft.speed(-(speed)-adj);
00171     MotorRight.speed(-(speed));
00172 }
00173 
00174 void turn_left(float speed) {
00175     //printf("Turning left\n");
00176     MotorLeft.speed(speed);
00177     MotorRight.speed(-(speed));
00178 }
00179 
00180 void turn_right(float speed) {
00181     //printf("Turning right\n");
00182     MotorLeft.speed(-(speed));
00183     MotorRight.speed(speed);
00184 }
00185 
00186 void reverseandturn(float speed) {
00187     //printf("Reverse and turn\n");
00188     MotorLeft.speed((speed-0.3));
00189     MotorRight.speed(-(speed-0.1));
00190 }
00191 
00192 void forwards(float speed, float adj) {
00193     MotorLeft.speed(speed+adj);
00194     MotorRight.speed(speed);
00195 }
00196 
00197 void move_random(float speed)
00198 {
00199     int counter;
00200     int fwd_bck;
00201     int fwd;
00202     float random_time;
00203 
00204     //printf("Moving randomly\n");
00205     counter = rand() % 5;
00206 
00207     for (int i = 0; i < counter; i++)
00208     {
00209         random_time = rand() % 1000 + 1000;
00210 
00211         fwd_bck = rand()%2;
00212         
00213         // If fwd_back == 1 move forward or backwards
00214         if (fwd_bck == 1) {
00215             fwd = rand()%2;
00216             // If fwd == 1 move forward
00217             if  (fwd == 1) { 
00218                 //printf("Moving forward\n");
00219                 move_detect(speed, fwd_bck, random_time);
00220             // If fwd == 0 move bacward
00221             } else {
00222                 //printf("Moving bacwards\n");
00223                 move_detect(speed, fwd_bck, random_time);
00224             }   
00225         // Turn
00226         } else {
00227             //printf("Turning\n");
00228             move_detect(speed, fwd_bck, random_time);
00229         }
00230     }
00231 }
00232 
00233 void stop() {
00234     MotorLeft.speed(0.0);
00235     MotorRight.speed(0.0);
00236 }
00237 
00238 int detect_object(int range_t, float speed) {
00239     // Start a timer - finds an object for 5 seconds
00240     // if it doesn't find anything returns 0
00241     Timer usensor_t, inner_t;
00242     usensor_t.start();
00243     
00244     // Variable to store sensed value
00245     unsigned int sense, dist, reverse;
00246     sense = 0;
00247     dist = 0;
00248     reverse = 0;
00249     
00250     while (usensor_t.read_ms() < 10000) {
00251         // Start the ultrasonic sensor
00252         usensor.start();
00253         inner_t.start();
00254         dist = usensor.get_dist_cm();
00255         
00256         printf("UltraSonic readings: %d \n", dist);
00257         
00258         // If an object is detected based on out set range return 1
00259         if (dist <= range_t && dist >= 1) {
00260             sense = 1;
00261             stop();
00262             flash_leds();
00263             break;
00264         } else {
00265             sense = 0;
00266             turn_left(speed);
00267         }
00268         
00269         if (inner_t.read_ms() >=100) {
00270             if (reverse == 2) {
00271                 speed = 0.7;
00272                 reverse = 0;
00273             } else {
00274                 speed = 0.0;
00275             }
00276             reverse++;
00277             inner_t.reset();
00278         }
00279     }
00280     
00281     usensor_t.stop();
00282     usensor_t.reset();
00283     return sense;
00284 }
00285 
00286 void move_detect(float speed, int fwd_bck, int time, int wait) {
00287     Timer t;
00288     t.start();
00289     int detect = 0;
00290     
00291     if(fwd_bck == 1) {
00292         forwards(speed);
00293         wait_ms(wait);
00294     } else {
00295         turn_left(speed + 0.2);
00296     }
00297     
00298     while (t.read_ms() < time) {
00299         detect = detect_line();
00300         // If line is detected from front then reverse
00301         if(detect == 1) {
00302             stop();
00303             turn_leds_on();
00304             move_detect(-forwardspeed,1,1000,300);
00305             stop();
00306             break;
00307         // If line is detected from back just keep on moving forward
00308         } else if (detect == -1) {
00309             stop();
00310             turn_leds_on();
00311             move_detect(forwardspeed,1,1000,300);
00312             stop();
00313             break;
00314         }
00315     }
00316     stop();
00317     t.stop();
00318 }