Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
autocar/autocar.cpp
- Committer:
- cudaChen
- Date:
- 2018-06-30
- Revision:
- 12:e95ed962be7a
- Child:
- 13:87cd0ae37e06
File content as of revision 12:e95ed962be7a:
#include "mbed.h" #include "autocar.h" 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); // used for output message via serial Serial pc(SERIAL_TX, SERIAL_RX); 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 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); }