Diff: main.cpp

Revision:

0:bcc2a42d221f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 11 01:54:21 2014 +0000
@@ -0,0 +1,80 @@
+#include "mbed.h"
+#include "Motor.h"
+
+//#define DEBUG 1
+
+#define SENSOR_L    p30
+#define SENSOR_R    p29
+#define MOTOR_L_PWM p26
+#define MOTOR_L_FWD p25
+#define MOTOR_L_REV p24
+#define MOTOR_R_PWM p21
+#define MOTOR_R_FWD p23
+#define MOTOR_R_REV p22
+#define BASE_SPEED  0.4
+
+DigitalOut myled(LED1);
+DigitalIn leftSensor (SENSOR_L);
+DigitalIn rightSensor(SENSOR_R);
+Motor       leftMotor (MOTOR_L_PWM, MOTOR_L_FWD, MOTOR_L_REV);
+Motor       rightMotor(MOTOR_R_PWM, MOTOR_R_FWD, MOTOR_R_REV);
+
+void backup(void);
+void rotate_left(void);
+void rotate_right(void);
+void turn_around(void);
+
+int main() {
+    //Set motors to forward
+    leftMotor.speed(BASE_SPEED);
+    rightMotor.speed(BASE_SPEED);
+    
+    while(1) {
+        #ifdef DEBUG
+        printf("Sensor Check:\r\n");
+        printf("Left  %d\r\n", left_test.read());
+        printf("Right %d\r\n", right_test.read());
+        #endif
+        
+
+        if(leftSensor & rightSensor){
+            backup();
+            turn_around();
+        }else if(leftSensor){
+            backup();
+            rotate_right();
+        }else if(rightSensor){
+            backup();
+            rotate_left();
+        }
+        
+        leftMotor.speed(BASE_SPEED);
+        rightMotor.speed(BASE_SPEED);
+        myled = 1;
+        wait(0.2);
+        myled = 0;
+        wait(.1);
+    }
+}
+
+
+void backup(void){
+    leftMotor.speed (-1*BASE_SPEED);
+    rightMotor.speed(-1*BASE_SPEED);
+    wait(.5);
+}
+void rotate_left(void){
+    leftMotor.speed (-1*BASE_SPEED);
+    rightMotor.speed(BASE_SPEED);
+    wait(1);
+}
+void rotate_right(void){
+    leftMotor.speed(BASE_SPEED);
+    rightMotor.speed(-1*BASE_SPEED);
+    wait(1);
+}
+void turn_around(void){
+    leftMotor.speed(BASE_SPEED);
+    rightMotor.speed(-1*BASE_SPEED);
+    wait(2);
+}
\ No newline at end of file