Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp@2:e0a553b64315, 2018-06-29 (annotated)
- Committer:
- cudaChen
- Date:
- Fri Jun 29 05:21:46 2018 +0000
- Revision:
- 2:e0a553b64315
- Parent:
- 1:ecbf974d0733
- Child:
- 3:812a72bbd664
fix typo of threshold
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 | 2:e0a553b64315 | 40 | |
cudaChen | 2:e0a553b64315 | 41 | // here I use 500 as threshold |
cudaChen | 2:e0a553b64315 | 42 | int threshold = 500; |
cudaChen | 0:190c4784b6f4 | 43 | |
cudaChen | 2:e0a553b64315 | 44 | while (true) { |
cudaChen | 2:e0a553b64315 | 45 | // not on track: > 500 |
cudaChen | 2:e0a553b64315 | 46 | // on track (black): < 500 |
cudaChen | 2:e0a553b64315 | 47 | left = leftIR.read_u16() < threshold ? true : false; |
cudaChen | 2:e0a553b64315 | 48 | middle = middleIR.read_u16() < threshold ? true : false; |
cudaChen | 2:e0a553b64315 | 49 | right = rightIR.read_u16() < threshold ? true : false; |
cudaChen | 0:190c4784b6f4 | 50 | |
cudaChen | 0:190c4784b6f4 | 51 | led1 = 1; // light on on-board LED |
cudaChen | 0:190c4784b6f4 | 52 | |
cudaChen | 0:190c4784b6f4 | 53 | // if middle detects black |
cudaChen | 0:190c4784b6f4 | 54 | if(middle) { |
cudaChen | 0:190c4784b6f4 | 55 | if(left && right) { // cliff |
cudaChen | 0:190c4784b6f4 | 56 | // stop |
cudaChen | 0:190c4784b6f4 | 57 | pc.printf("cliff\r\n"); |
cudaChen | 0:190c4784b6f4 | 58 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 59 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 60 | // pause for 0.5 sec |
cudaChen | 2:e0a553b64315 | 61 | wait_ms(100); |
cudaChen | 0:190c4784b6f4 | 62 | |
cudaChen | 0:190c4784b6f4 | 63 | // then go backward |
cudaChen | 0:190c4784b6f4 | 64 | DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 65 | DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 66 | } else if(left) { // turn left |
cudaChen | 0:190c4784b6f4 | 67 | pc.printf("turn left\r\n"); |
cudaChen | 1:ecbf974d0733 | 68 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 1:ecbf974d0733 | 69 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 1:ecbf974d0733 | 70 | } else if(right) { // turn right |
cudaChen | 1:ecbf974d0733 | 71 | pc.printf("turn right\r\n"); |
cudaChen | 0:190c4784b6f4 | 72 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 73 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 74 | } else { // go straight |
cudaChen | 0:190c4784b6f4 | 75 | pc.printf("go straight\r\n"); |
cudaChen | 0:190c4784b6f4 | 76 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 77 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 78 | } |
cudaChen | 0:190c4784b6f4 | 79 | |
cudaChen | 0:190c4784b6f4 | 80 | //wait_ms(1000); |
cudaChen | 0:190c4784b6f4 | 81 | } |
cudaChen | 0:190c4784b6f4 | 82 | |
cudaChen | 0:190c4784b6f4 | 83 | // if all detects white, which means the car is out of track |
cudaChen | 1:ecbf974d0733 | 84 | if(!middle && !left && !right) { // go backward |
cudaChen | 0:190c4784b6f4 | 85 | pc.printf("out of track, go backward\r\n"); |
cudaChen | 0:190c4784b6f4 | 86 | DriveSingleMotor(MOTOR_M1, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 87 | DriveSingleMotor(MOTOR_M2, 2, DIR_BACKWARD); |
cudaChen | 0:190c4784b6f4 | 88 | //wait_ms(1000); |
cudaChen | 0:190c4784b6f4 | 89 | } |
cudaChen | 0:190c4784b6f4 | 90 | |
cudaChen | 0:190c4784b6f4 | 91 | pc.printf("pause\r\n"); |
cudaChen | 0:190c4784b6f4 | 92 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 93 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 0:190c4784b6f4 | 94 | led1 = 0; // turn of on-board LED |
cudaChen | 2:e0a553b64315 | 95 | wait_ms(100); |
cudaChen | 0:190c4784b6f4 | 96 | } |
cudaChen | 0:190c4784b6f4 | 97 | } |
cudaChen | 0:190c4784b6f4 | 98 | |
cudaChen | 0:190c4784b6f4 | 99 | // m: 0 => M1 1 => M2 |
cudaChen | 0:190c4784b6f4 | 100 | // power 0~10 dir: 0=>正向 1=>反向 |
cudaChen | 0:190c4784b6f4 | 101 | void DriveSingleMotor(int m, int speed, int dir) |
cudaChen | 0:190c4784b6f4 | 102 | { |
cudaChen | 0:190c4784b6f4 | 103 | /*int _speed = 0; |
cudaChen | 0:190c4784b6f4 | 104 | |
cudaChen | 0:190c4784b6f4 | 105 | // 設定速度 |
cudaChen | 0:190c4784b6f4 | 106 | if (speed>10) speed=10; |
cudaChen | 0:190c4784b6f4 | 107 | _speed = map(speed, 1, 10, 100, 255); |
cudaChen | 0:190c4784b6f4 | 108 | |
cudaChen | 0:190c4784b6f4 | 109 | if (speed<=0) { |
cudaChen | 0:190c4784b6f4 | 110 | speed=0; |
cudaChen | 0:190c4784b6f4 | 111 | _speed=0; |
cudaChen | 0:190c4784b6f4 | 112 | } else |
cudaChen | 0:190c4784b6f4 | 113 | _speed = map(speed, 1, 10, 100, 255);*/ |
cudaChen | 0:190c4784b6f4 | 114 | |
cudaChen | 0:190c4784b6f4 | 115 | if (m == MOTOR_M1) { |
cudaChen | 0:190c4784b6f4 | 116 | // 設定方向 |
cudaChen | 0:190c4784b6f4 | 117 | if (speed == PWR_STOP) { |
cudaChen | 0:190c4784b6f4 | 118 | M1_in1 = 0; |
cudaChen | 0:190c4784b6f4 | 119 | M1_in2 = 0; |
cudaChen | 0:190c4784b6f4 | 120 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:190c4784b6f4 | 121 | // right wheel go forward |
cudaChen | 0:190c4784b6f4 | 122 | M1_in1 = 1; |
cudaChen | 0:190c4784b6f4 | 123 | M1_in2 = 0; |
cudaChen | 0:190c4784b6f4 | 124 | } else { |
cudaChen | 0:190c4784b6f4 | 125 | // right wheel go backward |
cudaChen | 0:190c4784b6f4 | 126 | M1_in1 = 0; |
cudaChen | 0:190c4784b6f4 | 127 | M1_in2 = 1; |
cudaChen | 0:190c4784b6f4 | 128 | } |
cudaChen | 0:190c4784b6f4 | 129 | //M1_enable.write_u16(_speed); // 驅動馬達 右輪 |
cudaChen | 0:190c4784b6f4 | 130 | M1_enable.write(1); |
cudaChen | 0:190c4784b6f4 | 131 | |
cudaChen | 0:190c4784b6f4 | 132 | } else if (m == MOTOR_M2) { |
cudaChen | 0:190c4784b6f4 | 133 | // 設定方向 |
cudaChen | 0:190c4784b6f4 | 134 | if (speed == PWR_STOP) { |
cudaChen | 0:190c4784b6f4 | 135 | M2_in3 = 0; |
cudaChen | 0:190c4784b6f4 | 136 | M2_in4 = 0; |
cudaChen | 0:190c4784b6f4 | 137 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:190c4784b6f4 | 138 | //左輪前進 |
cudaChen | 0:190c4784b6f4 | 139 | M2_in3 = 1; |
cudaChen | 0:190c4784b6f4 | 140 | M2_in4 = 0; |
cudaChen | 0:190c4784b6f4 | 141 | } else { |
cudaChen | 0:190c4784b6f4 | 142 | //左輪倒轉 |
cudaChen | 0:190c4784b6f4 | 143 | M2_in3 = 0; |
cudaChen | 0:190c4784b6f4 | 144 | M2_in4 = 1; |
cudaChen | 0:190c4784b6f4 | 145 | } |
cudaChen | 0:190c4784b6f4 | 146 | //M2_enable.write_u16(_speed); // 驅動馬達 左輪 |
cudaChen | 0:190c4784b6f4 | 147 | M2_enable.write(1); |
cudaChen | 0:190c4784b6f4 | 148 | } else { |
cudaChen | 0:190c4784b6f4 | 149 | //M1_enable.write_u16(PWR_STOP); // right wheel |
cudaChen | 0:190c4784b6f4 | 150 | //M2_enable.write_u16(PWR_STOP); // left wheel |
cudaChen | 0:190c4784b6f4 | 151 | M1_enable.write(0); |
cudaChen | 0:190c4784b6f4 | 152 | M2_enable.write(0); |
cudaChen | 0:190c4784b6f4 | 153 | } |
cudaChen | 0:190c4784b6f4 | 154 | } |
cudaChen | 0:190c4784b6f4 | 155 |