Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp@1:ecbf974d0733, 2018-06-28 (annotated)
- Committer:
- cudaChen
- Date:
- Thu Jun 28 07:30:44 2018 +0000
- Revision:
- 1:ecbf974d0733
- Parent:
- 0:190c4784b6f4
- Child:
- 2:e0a553b64315
fix some typo of turn left, right, and out of track
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cudaChen | 0:190c4784b6f4 | 1 | #include "mbed.h" |
cudaChen | 0:190c4784b6f4 | 2 | |
cudaChen | 0:190c4784b6f4 | 3 | DigitalOut led1(LED1); |
cudaChen | 0:190c4784b6f4 | 4 | |
cudaChen | 0:190c4784b6f4 | 5 | Serial pc(SERIAL_TX, SERIAL_RX); |
cudaChen | 0:190c4784b6f4 | 6 | |
cudaChen | 0:190c4784b6f4 | 7 | // used for motor |
cudaChen | 0:190c4784b6f4 | 8 | //AnalogOut M1_enable(D6); |
cudaChen | 0:190c4784b6f4 | 9 | //AnalogOut M2_enable(D5); |
cudaChen | 0:190c4784b6f4 | 10 | DigitalOut M1_enable(D6); |
cudaChen | 0:190c4784b6f4 | 11 | DigitalOut M2_enable(D5); |
cudaChen | 0:190c4784b6f4 | 12 | DigitalOut M1_in1(D12); |
cudaChen | 0:190c4784b6f4 | 13 | DigitalOut M1_in2(D11); |
cudaChen | 0:190c4784b6f4 | 14 | DigitalOut M2_in3(D10); |
cudaChen | 0:190c4784b6f4 | 15 | DigitalOut M2_in4(D9); |
cudaChen | 0:190c4784b6f4 | 16 | const int MOTOR_M1=0; |
cudaChen | 0:190c4784b6f4 | 17 | const int MOTOR_M2=1; |
cudaChen | 0:190c4784b6f4 | 18 | const int DIR_FORWARD= 0; |
cudaChen | 0:190c4784b6f4 | 19 | const int DIR_BACKWARD=1; |
cudaChen | 0:190c4784b6f4 | 20 | const int PWR_STOP = 0; |
cudaChen | 0:190c4784b6f4 | 21 | |
cudaChen | 0:190c4784b6f4 | 22 | // used for IR sensors |
cudaChen | 0:190c4784b6f4 | 23 | AnalogIn leftIR(A1); |
cudaChen | 0:190c4784b6f4 | 24 | AnalogIn middleIR(A3); |
cudaChen | 0:190c4784b6f4 | 25 | AnalogIn rightIR(A5); |
cudaChen | 0:190c4784b6f4 | 26 | |
cudaChen | 0:190c4784b6f4 | 27 | long map(long x, long in_min, long in_max, long out_min, long out_max) |
cudaChen | 0:190c4784b6f4 | 28 | { |
cudaChen | 0:190c4784b6f4 | 29 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cudaChen | 0:190c4784b6f4 | 30 | } |
cudaChen | 0:190c4784b6f4 | 31 | |
cudaChen | 0:190c4784b6f4 | 32 | void DriveSingleMotor(int m, int speed, int dir); |
cudaChen | 0:190c4784b6f4 | 33 | |
cudaChen | 0:190c4784b6f4 | 34 | // main() runs in its own thread in the OS |
cudaChen | 0:190c4784b6f4 | 35 | int main() |
cudaChen | 0:190c4784b6f4 | 36 | { |
cudaChen | 0:190c4784b6f4 | 37 | bool left = false; |
cudaChen | 0:190c4784b6f4 | 38 | bool middle = false; |
cudaChen | 0:190c4784b6f4 | 39 | bool right = false; |
cudaChen | 0:190c4784b6f4 | 40 | |
cudaChen | 0:190c4784b6f4 | 41 | while (true) { |
cudaChen | 0:190c4784b6f4 | 42 | // here I use 500 as threshold |
cudaChen | 0:190c4784b6f4 | 43 | // black: > 500 |
cudaChen | 0:190c4784b6f4 | 44 | // white: < 500 |
cudaChen | 0:190c4784b6f4 | 45 | left = leftIR.read_u16() > 500 ? true : false; |
cudaChen | 0:190c4784b6f4 | 46 | middle = middleIR.read_u16() > 500 ? true : false; |
cudaChen | 0:190c4784b6f4 | 47 | right = rightIR.read_u16() > 500 ? true : false; |
cudaChen | 0:190c4784b6f4 | 48 | |
cudaChen | 0:190c4784b6f4 | 49 | led1 = 1; // light on on-board LED |
cudaChen | 0:190c4784b6f4 | 50 | |
cudaChen | 0:190c4784b6f4 | 51 | // if middle detects black |
cudaChen | 0:190c4784b6f4 | 52 | if(middle) { |
cudaChen | 0:190c4784b6f4 | 53 | if(left && right) { // cliff |
cudaChen | 0:190c4784b6f4 | 54 | // stop |
cudaChen | 0:190c4784b6f4 | 55 | pc.printf("cliff\r\n"); |
cudaChen | 0:190c4784b6f4 | 56 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 57 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 58 | // pause for 0.5 sec |
cudaChen | 0:190c4784b6f4 | 59 | wait_ms(1000); |
cudaChen | 0:190c4784b6f4 | 60 | |
cudaChen | 0:190c4784b6f4 | 61 | // then go backward |
cudaChen | 0:190c4784b6f4 | 62 | DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 63 | DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 64 | } else if(left) { // turn left |
cudaChen | 0:190c4784b6f4 | 65 | pc.printf("turn left\r\n"); |
cudaChen | 1:ecbf974d0733 | 66 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 1:ecbf974d0733 | 67 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 1:ecbf974d0733 | 68 | } else if(right) { // turn right |
cudaChen | 1:ecbf974d0733 | 69 | pc.printf("turn right\r\n"); |
cudaChen | 0:190c4784b6f4 | 70 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 71 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 72 | } else { // go straight |
cudaChen | 0:190c4784b6f4 | 73 | pc.printf("go straight\r\n"); |
cudaChen | 0:190c4784b6f4 | 74 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 75 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 76 | } |
cudaChen | 0:190c4784b6f4 | 77 | |
cudaChen | 0:190c4784b6f4 | 78 | //wait_ms(1000); |
cudaChen | 0:190c4784b6f4 | 79 | } |
cudaChen | 0:190c4784b6f4 | 80 | |
cudaChen | 0:190c4784b6f4 | 81 | // if all detects white, which means the car is out of track |
cudaChen | 1:ecbf974d0733 | 82 | if(!middle && !left && !right) { // go backward |
cudaChen | 0:190c4784b6f4 | 83 | pc.printf("out of track, go backward\r\n"); |
cudaChen | 0:190c4784b6f4 | 84 | DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 85 | DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 86 | //wait_ms(1000); |
cudaChen | 0:190c4784b6f4 | 87 | } |
cudaChen | 0:190c4784b6f4 | 88 | |
cudaChen | 0:190c4784b6f4 | 89 | pc.printf("pause\r\n"); |
cudaChen | 0:190c4784b6f4 | 90 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 91 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 92 | led1 = 0; // turn of on-board LED |
cudaChen | 0:190c4784b6f4 | 93 | wait_ms(1000); |
cudaChen | 0:190c4784b6f4 | 94 | } |
cudaChen | 0:190c4784b6f4 | 95 | } |
cudaChen | 0:190c4784b6f4 | 96 | |
cudaChen | 0:190c4784b6f4 | 97 | // m: 0 => M1 1 => M2 |
cudaChen | 0:190c4784b6f4 | 98 | // power 0~10 dir: 0=>正向 1=>反向 |
cudaChen | 0:190c4784b6f4 | 99 | void DriveSingleMotor(int m, int speed, int dir) |
cudaChen | 0:190c4784b6f4 | 100 | { |
cudaChen | 0:190c4784b6f4 | 101 | /*int _speed = 0; |
cudaChen | 0:190c4784b6f4 | 102 | |
cudaChen | 0:190c4784b6f4 | 103 | // 設定速度 |
cudaChen | 0:190c4784b6f4 | 104 | if (speed>10) speed=10; |
cudaChen | 0:190c4784b6f4 | 105 | _speed = map(speed, 1, 10, 100, 255); |
cudaChen | 0:190c4784b6f4 | 106 | |
cudaChen | 0:190c4784b6f4 | 107 | if (speed<=0) { |
cudaChen | 0:190c4784b6f4 | 108 | speed=0; |
cudaChen | 0:190c4784b6f4 | 109 | _speed=0; |
cudaChen | 0:190c4784b6f4 | 110 | } else |
cudaChen | 0:190c4784b6f4 | 111 | _speed = map(speed, 1, 10, 100, 255);*/ |
cudaChen | 0:190c4784b6f4 | 112 | |
cudaChen | 0:190c4784b6f4 | 113 | if (m == MOTOR_M1) { |
cudaChen | 0:190c4784b6f4 | 114 | // 設定方向 |
cudaChen | 0:190c4784b6f4 | 115 | if (speed == PWR_STOP) { |
cudaChen | 0:190c4784b6f4 | 116 | M1_in1 = 0; |
cudaChen | 0:190c4784b6f4 | 117 | M1_in2 = 0; |
cudaChen | 0:190c4784b6f4 | 118 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:190c4784b6f4 | 119 | // right wheel go forward |
cudaChen | 0:190c4784b6f4 | 120 | M1_in1 = 1; |
cudaChen | 0:190c4784b6f4 | 121 | M1_in2 = 0; |
cudaChen | 0:190c4784b6f4 | 122 | } else { |
cudaChen | 0:190c4784b6f4 | 123 | // right wheel go backward |
cudaChen | 0:190c4784b6f4 | 124 | M1_in1 = 0; |
cudaChen | 0:190c4784b6f4 | 125 | M1_in2 = 1; |
cudaChen | 0:190c4784b6f4 | 126 | } |
cudaChen | 0:190c4784b6f4 | 127 | //M1_enable.write_u16(_speed); // 驅動馬達 右輪 |
cudaChen | 0:190c4784b6f4 | 128 | M1_enable.write(1); |
cudaChen | 0:190c4784b6f4 | 129 | |
cudaChen | 0:190c4784b6f4 | 130 | } else if (m == MOTOR_M2) { |
cudaChen | 0:190c4784b6f4 | 131 | // 設定方向 |
cudaChen | 0:190c4784b6f4 | 132 | if (speed == PWR_STOP) { |
cudaChen | 0:190c4784b6f4 | 133 | M2_in3 = 0; |
cudaChen | 0:190c4784b6f4 | 134 | M2_in4 = 0; |
cudaChen | 0:190c4784b6f4 | 135 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:190c4784b6f4 | 136 | //左輪前進 |
cudaChen | 0:190c4784b6f4 | 137 | M2_in3 = 1; |
cudaChen | 0:190c4784b6f4 | 138 | M2_in4 = 0; |
cudaChen | 0:190c4784b6f4 | 139 | } else { |
cudaChen | 0:190c4784b6f4 | 140 | //左輪倒轉 |
cudaChen | 0:190c4784b6f4 | 141 | M2_in3 = 0; |
cudaChen | 0:190c4784b6f4 | 142 | M2_in4 = 1; |
cudaChen | 0:190c4784b6f4 | 143 | } |
cudaChen | 0:190c4784b6f4 | 144 | //M2_enable.write_u16(_speed); // 驅動馬達 左輪 |
cudaChen | 0:190c4784b6f4 | 145 | M2_enable.write(1); |
cudaChen | 0:190c4784b6f4 | 146 | } else { |
cudaChen | 0:190c4784b6f4 | 147 | //M1_enable.write_u16(PWR_STOP); // right wheel |
cudaChen | 0:190c4784b6f4 | 148 | //M2_enable.write_u16(PWR_STOP); // left wheel |
cudaChen | 0:190c4784b6f4 | 149 | M1_enable.write(0); |
cudaChen | 0:190c4784b6f4 | 150 | M2_enable.write(0); |
cudaChen | 0:190c4784b6f4 | 151 | } |
cudaChen | 0:190c4784b6f4 | 152 | } |
cudaChen | 0:190c4784b6f4 | 153 |