main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "SRF05.h"
00004 
00005 Servo motor (p21);
00006 Servo turn (p22);
00007 
00008 SRF05 front (p24,p23);
00009 SRF05 back (p26,p25);
00010 
00011 DigitalOut b (p20);
00012 DigitalOut led1 (LED1);
00013 DigitalOut led2 (LED2);
00014 
00015 void backward (float);
00016 void forward (float);
00017 void right (void);
00018 void left (void);
00019 void reset (void);
00020 void stop (void);
00021 
00022 
00023 int i = 0;
00024 int j = 0;
00025 
00026 float sample_delay = 0.01;
00027 
00028 int main() {
00029 
00030     b = 0;
00031     reset();
00032 
00033     //main loop
00034     while (1) {
00035 
00036         //init
00037         i = front.read();
00038         j = back.read();
00039 
00040         
00041            //can go forward?
00042         if (i > 100) {
00043             while (i > 100) {
00044 
00045                 forward(20);
00046                 i = front.read();
00047                 j = back.read();
00048             }
00049 
00050             stop();
00051         }
00052         
00053         //can it turn to avoid?
00054         if (i > 10 && i < 100) {
00055             led1 = 1;
00056             while (i > 10 && i < 100) {
00057                 
00058                 forward(20);
00059                 turn.write(0);
00060                 
00061                 i = front.read();
00062                 j = back.read();
00063             }
00064             led1 = 0;
00065             stop();
00066         }
00067         
00068         //is there room to ververse and turn?
00069         if (i < 20 && j > 10) {
00070         led2 = 1;
00071             while (i < 20 && j > 10) {
00072             
00073                 backward(20);
00074                 turn.write(1);
00075                 
00076                 i = front.read();
00077                 j = back.read();
00078             }
00079             led2 = 0;
00080             stop();
00081         }
00082     }
00083 }
00084 
00085 void reset (void) {
00086     turn.write(0.5);
00087     motor.write(0.48);
00088 }
00089 
00090 // 0.5 = 0 speed & 0 = full speed
00091 void forward (float speed) {
00092     float y = speed / 100;
00093     float x = y * 0.5;
00094     float z = 0.5 - x;
00095 
00096     motor.write(z);
00097 }
00098 
00099 // 0.5 = 0 speed & 1 = full speed
00100 
00101 void backward (float speed) {
00102     float y = speed / 100;
00103     float x = y * 0.5;
00104     float z = 0.5 + x;
00105 
00106     motor.write(z);
00107 }
00108 
00109 // 0 = right 0.5 = straight 1 = left
00110 void left (void) {
00111     turn.write(1);
00112 }
00113 void right (void) {
00114     turn.write(0);
00115 }
00116 
00117 void stop (void) {
00118     b = 1;
00119     wait(0.3);
00120     reset();
00121     wait(0.1);
00122     b = 0;
00123     wait(1);
00124 }