Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp
- Committer:
- cudaChen
- Date:
- 2018-06-30
- Revision:
- 9:ce1dd89ff5e8
- Parent:
- 7:4edd049209a6
- Child:
- 10:a14380381d86
File content as of revision 9:ce1dd89ff5e8:
#include "mbed.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); 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); void driveMotor(bool left, bool middle, bool right); // main() runs in its own thread in the OS int main() { bool left = false; bool middle = false; bool right = false; // here I use 500 as threshold int threshold = 500; while (true) { int reading1 = pb.read(); if(reading1 != lastButtonState) { wait_ms(20); int reading2 = pb.read(); if(reading2 == reading1) { lastButtonState = reading2; } if(lastButtonState == 1) { ledState = !ledState; } } led1.write(ledState); 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; driveMotor(left, middle, right); } } } // 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"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); break; case 6: // turn left pc.printf("6. turn left\r\n"); DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); break; case 5: // go straight pc.printf("5. go straight\r\n"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); break; case 4: // turn left pc.printf("4. turn left\r\n"); DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); break; case 3: // turn right pc.printf("3. turn right\r\n"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); break; case 2: // go straight pc.printf("2. go straight\r\n"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); break; case 1: // turn right pc.printf("1. turn right\r\n"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); break; case 0: // go straight pc.printf("0. go straight\r\n"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); break; default: pc.printf("invalid\r\n"); break; } }