Diff: main.cpp

Revision:

12:e95ed962be7a

Parent:

11:3e9d4c345ebd

Child:

13:87cd0ae37e06

--- a/main.cpp	Sat Jun 30 12:41:24 2018 +0000
+++ b/main.cpp	Sat Jun 30 13:08:00 2018 +0000
@@ -1,47 +1,13 @@
 #include "mbed.h"
 
+#include "autocar.h"
+
 // trigger button and triggered LED
 DigitalOut led1(LED1);
 DigitalIn pb(PC_13);
 int lastButtonState = 0;
 bool ledState = false;
 
-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(D3);
-DigitalOut M1_in2(D2);
-DigitalOut M2_in3(D1);
-DigitalOut M2_in4(D0);
-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);
-void readIR(bool* left, bool* middle, bool* right, int threshold);
-
-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;
-}
-
-// used for controlling the direction of auto car
-void DriveSingleMotor(int m, int speed, int dir);
-void driveMotor(bool left, bool middle, bool right);
-void init();
-void forward();
-void turnLeft();
-void turnRight();
-
 // main() runs in its own thread in the OS
 int main()
 {
@@ -77,146 +43,9 @@
         if(ledState) {
             // not on track: > 500
             // on track (black): < 500
-            /*left = leftIR.read_u16() < threshold ? true : false;
-            middle = middleIR.read_u16() < threshold ? true : false;
-            right = rightIR.read_u16() < threshold ? true : false;*/
             readIR(&left, &middle, &right, threshold);
 
             driveMotor(left, middle, right);
         }
     }
 }
-
-void readIR(bool* left, bool* middle, bool* right, int threshold)
-{
-    // not on track: > 500
-    // on track (black): < 500
-    *left = leftIR.read_u16() < threshold ? true : false;
-    *middle = middleIR.read_u16() < threshold ? true : false;
-    *right = rightIR.read_u16() < threshold ? true : false;
-}
-
-// 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);
-    }
-}
-
-void driveMotor(bool left, bool middle, bool right)
-{
-    int status = left * 4 + middle * 2 + right;
-    switch(status) {
-        case 7: // go straight
-            pc.printf("7. go straight\r\n");
-            forward();
-            break;
-        case 6: // turn left
-            pc.printf("6. turn left\r\n");
-            turnLeft();
-            break;
-        case 5: // go straight
-            pc.printf("5. go straight\r\n");
-            forward();
-            break;
-        case 4: // turn left
-            pc.printf("4. turn left\r\n");
-            turnLeft();
-            break;
-        case 3: // turn right
-            pc.printf("3. turn right\r\n");
-            turnRight();
-            break;
-        case 2: // go straight
-            pc.printf("2. go straight\r\n");
-            forward();
-            break;
-        case 1: // turn right
-            pc.printf("1. turn right\r\n");
-            turnRight();
-            break;
-        case 0: // go straight
-            pc.printf("0. go straight\r\n");
-            forward();
-            break;
-        default:
-            pc.printf("invalid\r\n");
-            break;
-    }
-}
-
-void init()
-{
-    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
-    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
-}
-
-void forward()
-{
-    DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-    DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
-}
-
-void turnLeft()
-{
-    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
-    DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
-}
-
-void turnRight()
-{
-    DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
-}
-
-