Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
Diff: autocar/autocar.cpp
- Revision:
- 12:e95ed962be7a
- Child:
- 13:87cd0ae37e06
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/autocar/autocar.cpp Sat Jun 30 13:08:00 2018 +0000 @@ -0,0 +1,160 @@ +#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); +} \ No newline at end of file