mainV5.cpp

00001 #include "mbed.h"
00002 #include "Brobot.h"
00003 #include "IRSensor.h"
00004 #include "PID_Control.h"
00005 
00006 DigitalOut led(LED1);
00007 
00008 //Perophery for distance sensors
00009 AnalogIn distance(PB_1);
00010 DigitalIn button(USER_BUTTON);
00011 DigitalOut enable(PC_1);
00012 DigitalOut bit0(PH_1);
00013 DigitalOut bit1(PC_2);
00014 DigitalOut bit2(PC_3);
00015 IRSensor sensors[6];
00016 
00017 //indicator leds arround robot
00018 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
00019 
00020 //defining the sensors for
00021 IRSensor sensor_front( &distance, &bit0, &bit1, &bit2, 0);
00022 IRSensor sensor_left( &distance, &bit0, &bit1, &bit2, 5);
00023 IRSensor sensor_right( &distance, &bit0, &bit1, &bit2, 1);
00024 
00025 //timer object for ledshow and distance sensor
00026 Ticker t1;
00027 
00028 //motor stuff
00029 DigitalOut enableMotorDriver(PB_2);
00030 PwmOut pwmL(PA_8);
00031 PwmOut pwmR(PA_9);
00032 int number=0;
00033 PID_Control pid;
00034 
00035 Brobot stingray(&pwmL, &pwmR, number);
00036 
00037 //lights up the led according to the sensor signal
00038 void ledDistance()
00039 {
00040     for( int ii = 0; ii<6; ++ii)
00041         sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0;   // an if statement in one line
00042 }
00043 
00044 //blinks at startup and starts ledDistance task
00045 void ledShow()
00046 {
00047     static int timer = 0;   // static is only the first time/loop
00048     for( int ii = 0; ii<6; ++ii)
00049         leds[ii] = !leds[ii];
00050 
00051     //quit ticker and start led distance show
00052     if( ++timer > 10) {
00053         t1.detach();
00054         t1.attach( &ledDistance, 0.01f );
00055     }
00056 }
00057 
00058 //initiate PWM with period and enable
00059 void initPWM()
00060 {
00061     pwmL.period(0.00005f);
00062     pwmR.period(0.00005f);
00063     pwmL = 0.5f;
00064     pwmR = 0.5f;
00065     enableMotorDriver =1;
00066 }
00067 
00068 // Collision avoidance
00069 void move()
00070 {
00071     wait(0.01);
00072     float speed = 0.25;
00073 
00074     float e = sensor_left.read() - sensor_right.read();
00075     float diff = pid.calc( 0.0f+e, 0.005f );
00076 
00077     pwmL = 0.5f - diff + speed;
00078     pwmR = 0.5f - diff - speed;
00079 
00080     if(sensor_front.read() <=0.3f) { // when approaching normal to wall
00081         stingray.stop();
00082         int direction=rand()%2 ;    // so there is variablility in the robots path
00083         if(direction==0) {
00084             stingray.rotate(0.1f);   // and value between 0 and 0.25
00085             wait(0.5);
00086         } else if (direction==1) {
00087             stingray.rotate(-0.1f);
00088             wait(0.5);
00089         }
00090     }
00091 }
00092 
00093 int main()
00094 {
00095     enableMotorDriver = 0;  //safety precaution
00096     enable = 1 ;            //for debugging and output
00097 
00098     t1.attach( &ledShow, 0.05f );  //ticker
00099 
00100     enum robot_states {idle=0, search};   //so that we can reference states by name vs number
00101     int robot_state=idle;    // define and start in idle state
00102 
00103     //initialize distance sensors
00104     for( int ii = 0; ii<6; ++ii)
00105         sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
00106         
00107     //                  kp    ki     kd      min     max
00108     pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000);
00109 
00110     while(1) {
00111         wait( 0.1f ); //rquired to allow time for ticker
00112 
00113         switch(robot_state) {
00114             case idle:
00115                 if(button == 0) {       // 0 is pressed
00116                     initPWM();          // sets period
00117                     robot_state=search;         // next state
00118                 }
00119                 break;
00120 
00121             case search:
00122                 move();
00123                 break;
00124         }
00125     }
00126 }