Diff: mainV5.cpp

Revision:

0:01c109e18d49

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mainV5.cpp	Fri May 26 12:33:39 2017 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include "Brobot.h"
+#include "IRSensor.h"
+#include "PID_Control.h"
+
+DigitalOut led(LED1);
+
+//Perophery for distance sensors
+AnalogIn distance(PB_1);
+DigitalIn button(USER_BUTTON);
+DigitalOut enable(PC_1);
+DigitalOut bit0(PH_1);
+DigitalOut bit1(PC_2);
+DigitalOut bit2(PC_3);
+IRSensor sensors[6];
+
+//indicator leds arround robot
+DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
+
+//defining the sensors for
+IRSensor sensor_front( &distance, &bit0, &bit1, &bit2, 0);
+IRSensor sensor_left( &distance, &bit0, &bit1, &bit2, 5);
+IRSensor sensor_right( &distance, &bit0, &bit1, &bit2, 1);
+
+//timer object for ledshow and distance sensor
+Ticker t1;
+
+//motor stuff
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmL(PA_8);
+PwmOut pwmR(PA_9);
+int number=0;
+PID_Control pid;
+
+Brobot stingray(&pwmL, &pwmR, number);
+
+//lights up the led according to the sensor signal
+void ledDistance()
+{
+    for( int ii = 0; ii<6; ++ii)
+        sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0;   // an if statement in one line
+}
+
+//blinks at startup and starts ledDistance task
+void ledShow()
+{
+    static int timer = 0;   // static is only the first time/loop
+    for( int ii = 0; ii<6; ++ii)
+        leds[ii] = !leds[ii];
+
+    //quit ticker and start led distance show
+    if( ++timer > 10) {
+        t1.detach();
+        t1.attach( &ledDistance, 0.01f );
+    }
+}
+
+//initiate PWM with period and enable
+void initPWM()
+{
+    pwmL.period(0.00005f);
+    pwmR.period(0.00005f);
+    pwmL = 0.5f;
+    pwmR = 0.5f;
+    enableMotorDriver =1;
+}
+
+// Collision avoidance
+void move()
+{
+    wait(0.01);
+    float speed = 0.25;
+
+    float e = sensor_left.read() - sensor_right.read();
+    float diff = pid.calc( 0.0f+e, 0.005f );
+
+    pwmL = 0.5f - diff + speed;
+    pwmR = 0.5f - diff - speed;
+
+    if(sensor_front.read() <=0.3f) { // when approaching normal to wall
+        stingray.stop();
+        int direction=rand()%2 ;    // so there is variablility in the robots path
+        if(direction==0) {
+            stingray.rotate(0.1f);   // and value between 0 and 0.25
+            wait(0.5);
+        } else if (direction==1) {
+            stingray.rotate(-0.1f);
+            wait(0.5);
+        }
+    }
+}
+
+int main()
+{
+    enableMotorDriver = 0;  //safety precaution
+    enable = 1 ;            //for debugging and output
+
+    t1.attach( &ledShow, 0.05f );  //ticker
+
+    enum robot_states {idle=0, search};   //so that we can reference states by name vs number
+    int robot_state=idle;    // define and start in idle state
+
+    //initialize distance sensors
+    for( int ii = 0; ii<6; ++ii)
+        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
+        
+    //                  kp    ki     kd      min     max
+    pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000);
+
+    while(1) {
+        wait( 0.1f ); //rquired to allow time for ticker
+
+        switch(robot_state) {
+            case idle:
+                if(button == 0) {       // 0 is pressed
+                    initPWM();          // sets period
+                    robot_state=search;         // next state
+                }
+                break;
+
+            case search:
+                move();
+                break;
+        }
+    }
+}