Revision 3:581d391d248c, committed 2016-10-11

Comitter:

csellier

Date:

Tue Oct 11 14:19:12 2016 +0000

Parent:

2:0b86e6482e4b

Child:

4:c1f9aaf55e8b

Commit message:

updated program

Changed in this revision

--- a/main.cpp	Tue Oct 11 11:47:25 2016 +0000
+++ b/main.cpp	Tue Oct 11 14:19:12 2016 +0000
@@ -1,10 +1,17 @@
 #include "mbed.h"
 #include "Servo.h"
+
+#define DISTANCE 0.45
+
+
 Serial pc (USBTX, USBRX); // USB serial interface
+
 Servo ServoRight(p21); // continuous rotation hobby servo right
 Servo ServoLeft(p22); // continuous rotation hobby servo left
+
 PwmOut LEDleft(p23);
 PwmOut LEDright(p24);
+
 AnalogIn IRfront(p15);
 AnalogIn IRleft(p16);
 AnalogIn IRright(p17);
@@ -12,29 +19,50 @@
 AnalogIn LDRright(p19);
 
 void init_servo() {
- // calibrate the servos for +/-5ms over +/-45deg
- ServoRight.calibrate(0.0005,45);
- ServoLeft.calibrate(0.0005,45);
+    // calibrate the servos for +/-5ms over +/-45deg
+    ServoRight.calibrate(0.0005,45);
+    ServoLeft.calibrate(0.0005,45);
 }
-int main() {
 
+int main() {
+    float front;
+    float left;
+    float right;
 
- // set the USB serial interface baud rate
- pc.baud(921600);
-
- init_servo();
+     // set the USB serial interface baud rate
+     pc.baud(921600);
+     init_servo();
 
  while(1) {
+    
+    // Auto drive
+    front = IRfront.read();
+    left  = IRleft.read();
+    right = IRright.read();
+    
+    if (front > DISTANCE) {
+        if (left > DISTANCE) {
+            // turn right
+        } else {
+            // turn left    
+        }
+    } else if (left > DISTANCE) {
+        if (right < DISTANCE) {
+            // right
+        } else {
+            // forward    
+        }
+    }
+    
+    //Turn both LED's on for the line following feature
+    LEDleft=1;
+    LEDright=1;
+    
+    // sernd the servo command to the servos themselves
+    ServoRight.write(0.6); // write to the continuous rotation servo1
+    ServoLeft.write(0.4); // write to the continous servo2
 
- //Turn both LED's on for the line following feature
-  LEDleft=1;
-  LEDright=1;
-  
- // sernd the servo command to the servos themselves
- ServoRight.write(0.6); // write to the continuous rotation servo1
- ServoLeft.write(0.4); // write to the continous servo2
-
- }
+    }
 }