Diff: main.cpp

Revision:

0:190c4784b6f4

Child:

1:ecbf974d0733

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 27 16:23:33 2018 +0000
@@ -0,0 +1,153 @@
+#include "mbed.h"
+
+DigitalOut led1(LED1);
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+// used for motor
+//AnalogOut M1_enable(D6);
+//AnalogOut M2_enable(D5);
+DigitalOut M1_enable(D6);
+DigitalOut M2_enable(D5);
+DigitalOut M1_in1(D12);
+DigitalOut M1_in2(D11);
+DigitalOut M2_in3(D10);
+DigitalOut M2_in4(D9);
+const int MOTOR_M1=0;
+const int MOTOR_M2=1;
+const int DIR_FORWARD= 0;
+const int DIR_BACKWARD=1;
+const int PWR_STOP = 0;
+
+// used for IR sensors
+AnalogIn leftIR(A1);
+AnalogIn middleIR(A3);
+AnalogIn rightIR(A5);
+
+long map(long x, long in_min, long in_max, long out_min, long out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+void DriveSingleMotor(int m, int speed, int dir);
+
+// main() runs in its own thread in the OS
+int main()
+{
+    bool left = false;
+    bool middle = false;
+    bool right = false;
+
+    while (true) {
+        // here I use 500 as threshold
+        // black: > 500
+        // white: < 500
+        left = leftIR.read_u16() > 500 ? true : false;
+        middle = middleIR.read_u16() > 500 ? true : false;
+        right = rightIR.read_u16() > 500 ? true : false;
+
+        led1 = 1; // light on on-board LED
+
+        // if middle detects black
+        if(middle) {
+            if(left && right) { // cliff
+                // stop
+                pc.printf("cliff\r\n");
+                DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+                DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+                // pause for 0.5 sec
+                wait_ms(1000);
+
+                // then go backward
+                DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD);
+                DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD);
+            } else if(left) { // turn left
+                pc.printf("turn left\r\n");
+                DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+                DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+            } else if(right) { // turn right
+                pc.printf("turn right\r\n");
+                DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+                DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            } else { // go straight
+                pc.printf("go straight\r\n");
+                DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+                DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            }
+
+            //wait_ms(1000);
+        }
+
+        // if all detects white, which means the car is out of track
+        if(middle && left && right) { // go backward
+            pc.printf("out of track, go backward\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD);
+            //wait_ms(1000);
+        }
+
+        pc.printf("pause\r\n");
+        DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+        DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+        led1 = 0; // turn of on-board LED
+        wait_ms(1000);
+    }
+}
+
+// m: 0 => M1      1 => M2
+// power 0~10     dir: 0=>正向　1=>反向
+void DriveSingleMotor(int m, int speed, int dir)
+{
+    /*int _speed = 0;
+
+    //　設定速度
+    if (speed>10) speed=10;
+    _speed = map(speed, 1, 10, 100, 255);
+
+    if (speed<=0) {
+        speed=0;
+        _speed=0;
+    } else
+        _speed = map(speed, 1, 10, 100, 255);*/
+
+    if (m == MOTOR_M1) {
+        //　設定方向
+        if (speed == PWR_STOP) {
+            M1_in1 = 0;
+            M1_in2 = 0;
+        } else if (dir ==  DIR_FORWARD) {
+            // right wheel go forward
+            M1_in1 = 1;
+            M1_in2 = 0;
+        } else {
+            // right wheel go backward
+            M1_in1 = 0;
+            M1_in2 = 1;
+        }
+        //M1_enable.write_u16(_speed); // 驅動馬達  右輪
+        M1_enable.write(1);
+
+    } else if (m == MOTOR_M2) {
+        //　設定方向
+        if (speed == PWR_STOP) {
+            M2_in3 = 0;
+            M2_in4 = 0;
+        } else if (dir == DIR_FORWARD) {
+            //左輪前進
+            M2_in3 = 1;
+            M2_in4 = 0;
+        } else {
+            //左輪倒轉
+            M2_in3 = 0;
+            M2_in4 = 1;
+        }
+        //M2_enable.write_u16(_speed); // 驅動馬達  左輪
+        M2_enable.write(1);
+    } else {
+        //M1_enable.write_u16(PWR_STOP); // right wheel
+        //M2_enable.write_u16(PWR_STOP); // left wheel
+        M1_enable.write(0);
+        M2_enable.write(0);
+    }
+}
+