Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp
- Committer:
- cudaChen
- Date:
- 2018-06-28
- Revision:
- 1:ecbf974d0733
- Parent:
- 0:190c4784b6f4
- Child:
- 2:e0a553b64315
File content as of revision 1:ecbf974d0733:
#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, PWR_STOP, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); } else if(right) { // turn right pc.printf("turn right\r\n"); DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); DriveSingleMotor(MOTOR_M2, PWR_STOP, 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); } }