Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp@9:ce1dd89ff5e8, 2018-06-30 (annotated)
- Committer:
- cudaChen
- Date:
- Sat Jun 30 06:53:13 2018 +0000
- Revision:
- 9:ce1dd89ff5e8
- Parent:
- 7:4edd049209a6
- Child:
- 10:a14380381d86
change pin of motor control
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 | 7:4edd049209a6 | 3 | // trigger button and triggered LED |
cudaChen | 0:190c4784b6f4 | 4 | DigitalOut led1(LED1); |
cudaChen | 7:4edd049209a6 | 5 | DigitalIn pb(PC_13); |
cudaChen | 7:4edd049209a6 | 6 | int lastButtonState = 0; |
cudaChen | 7:4edd049209a6 | 7 | bool ledState = false; |
cudaChen | 0:190c4784b6f4 | 8 | |
cudaChen | 0:190c4784b6f4 | 9 | Serial pc(SERIAL_TX, SERIAL_RX); |
cudaChen | 0:190c4784b6f4 | 10 | |
cudaChen | 0:190c4784b6f4 | 11 | // used for motor |
cudaChen | 0:190c4784b6f4 | 12 | //AnalogOut M1_enable(D6); |
cudaChen | 0:190c4784b6f4 | 13 | //AnalogOut M2_enable(D5); |
cudaChen | 0:190c4784b6f4 | 14 | DigitalOut M1_enable(D6); |
cudaChen | 0:190c4784b6f4 | 15 | DigitalOut M2_enable(D5); |
cudaChen | 9:ce1dd89ff5e8 | 16 | DigitalOut M1_in1(D3); |
cudaChen | 9:ce1dd89ff5e8 | 17 | DigitalOut M1_in2(D2); |
cudaChen | 9:ce1dd89ff5e8 | 18 | DigitalOut M2_in3(D1); |
cudaChen | 9:ce1dd89ff5e8 | 19 | DigitalOut M2_in4(D0); |
cudaChen | 0:190c4784b6f4 | 20 | const int MOTOR_M1=0; |
cudaChen | 0:190c4784b6f4 | 21 | const int MOTOR_M2=1; |
cudaChen | 0:190c4784b6f4 | 22 | const int DIR_FORWARD= 0; |
cudaChen | 0:190c4784b6f4 | 23 | const int DIR_BACKWARD=1; |
cudaChen | 0:190c4784b6f4 | 24 | const int PWR_STOP = 0; |
cudaChen | 0:190c4784b6f4 | 25 | |
cudaChen | 0:190c4784b6f4 | 26 | // used for IR sensors |
cudaChen | 0:190c4784b6f4 | 27 | AnalogIn leftIR(A1); |
cudaChen | 0:190c4784b6f4 | 28 | AnalogIn middleIR(A3); |
cudaChen | 0:190c4784b6f4 | 29 | AnalogIn rightIR(A5); |
cudaChen | 0:190c4784b6f4 | 30 | |
cudaChen | 0:190c4784b6f4 | 31 | long map(long x, long in_min, long in_max, long out_min, long out_max) |
cudaChen | 0:190c4784b6f4 | 32 | { |
cudaChen | 0:190c4784b6f4 | 33 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cudaChen | 0:190c4784b6f4 | 34 | } |
cudaChen | 0:190c4784b6f4 | 35 | |
cudaChen | 0:190c4784b6f4 | 36 | void DriveSingleMotor(int m, int speed, int dir); |
cudaChen | 5:25faf509ee9b | 37 | void driveMotor(bool left, bool middle, bool right); |
cudaChen | 0:190c4784b6f4 | 38 | |
cudaChen | 0:190c4784b6f4 | 39 | // main() runs in its own thread in the OS |
cudaChen | 0:190c4784b6f4 | 40 | int main() |
cudaChen | 0:190c4784b6f4 | 41 | { |
cudaChen | 0:190c4784b6f4 | 42 | bool left = false; |
cudaChen | 0:190c4784b6f4 | 43 | bool middle = false; |
cudaChen | 0:190c4784b6f4 | 44 | bool right = false; |
cudaChen | 4:76b9213714cc | 45 | |
cudaChen | 2:e0a553b64315 | 46 | // here I use 500 as threshold |
cudaChen | 2:e0a553b64315 | 47 | int threshold = 500; |
cudaChen | 0:190c4784b6f4 | 48 | |
cudaChen | 4:76b9213714cc | 49 | while (true) { |
cudaChen | 7:4edd049209a6 | 50 | int reading1 = pb.read(); |
cudaChen | 7:4edd049209a6 | 51 | |
cudaChen | 7:4edd049209a6 | 52 | if(reading1 != lastButtonState) { |
cudaChen | 7:4edd049209a6 | 53 | wait_ms(20); |
cudaChen | 7:4edd049209a6 | 54 | |
cudaChen | 7:4edd049209a6 | 55 | int reading2 = pb.read(); |
cudaChen | 7:4edd049209a6 | 56 | |
cudaChen | 7:4edd049209a6 | 57 | if(reading2 == reading1) { |
cudaChen | 7:4edd049209a6 | 58 | lastButtonState = reading2; |
cudaChen | 7:4edd049209a6 | 59 | } |
cudaChen | 7:4edd049209a6 | 60 | |
cudaChen | 7:4edd049209a6 | 61 | if(lastButtonState == 1) { |
cudaChen | 7:4edd049209a6 | 62 | ledState = !ledState; |
cudaChen | 7:4edd049209a6 | 63 | } |
cudaChen | 7:4edd049209a6 | 64 | } |
cudaChen | 7:4edd049209a6 | 65 | |
cudaChen | 7:4edd049209a6 | 66 | led1.write(ledState); |
cudaChen | 7:4edd049209a6 | 67 | |
cudaChen | 7:4edd049209a6 | 68 | if(ledState) { |
cudaChen | 2:e0a553b64315 | 69 | // not on track: > 500 |
cudaChen | 2:e0a553b64315 | 70 | // on track (black): < 500 |
cudaChen | 2:e0a553b64315 | 71 | left = leftIR.read_u16() < threshold ? true : false; |
cudaChen | 2:e0a553b64315 | 72 | middle = middleIR.read_u16() < threshold ? true : false; |
cudaChen | 2:e0a553b64315 | 73 | right = rightIR.read_u16() < threshold ? true : false; |
cudaChen | 0:190c4784b6f4 | 74 | |
cudaChen | 5:25faf509ee9b | 75 | driveMotor(left, middle, right); |
cudaChen | 7:4edd049209a6 | 76 | } |
cudaChen | 0:190c4784b6f4 | 77 | } |
cudaChen | 0:190c4784b6f4 | 78 | } |
cudaChen | 0:190c4784b6f4 | 79 | |
cudaChen | 0:190c4784b6f4 | 80 | // m: 0 => M1 1 => M2 |
cudaChen | 0:190c4784b6f4 | 81 | // power 0~10 dir: 0=>正向 1=>反向 |
cudaChen | 0:190c4784b6f4 | 82 | void DriveSingleMotor(int m, int speed, int dir) |
cudaChen | 0:190c4784b6f4 | 83 | { |
cudaChen | 0:190c4784b6f4 | 84 | /*int _speed = 0; |
cudaChen | 0:190c4784b6f4 | 85 | |
cudaChen | 0:190c4784b6f4 | 86 | // 設定速度 |
cudaChen | 0:190c4784b6f4 | 87 | if (speed>10) speed=10; |
cudaChen | 0:190c4784b6f4 | 88 | _speed = map(speed, 1, 10, 100, 255); |
cudaChen | 0:190c4784b6f4 | 89 | |
cudaChen | 0:190c4784b6f4 | 90 | if (speed<=0) { |
cudaChen | 0:190c4784b6f4 | 91 | speed=0; |
cudaChen | 0:190c4784b6f4 | 92 | _speed=0; |
cudaChen | 0:190c4784b6f4 | 93 | } else |
cudaChen | 0:190c4784b6f4 | 94 | _speed = map(speed, 1, 10, 100, 255);*/ |
cudaChen | 0:190c4784b6f4 | 95 | |
cudaChen | 0:190c4784b6f4 | 96 | if (m == MOTOR_M1) { |
cudaChen | 0:190c4784b6f4 | 97 | // 設定方向 |
cudaChen | 0:190c4784b6f4 | 98 | if (speed == PWR_STOP) { |
cudaChen | 0:190c4784b6f4 | 99 | M1_in1 = 0; |
cudaChen | 0:190c4784b6f4 | 100 | M1_in2 = 0; |
cudaChen | 0:190c4784b6f4 | 101 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:190c4784b6f4 | 102 | // right wheel go forward |
cudaChen | 0:190c4784b6f4 | 103 | M1_in1 = 1; |
cudaChen | 0:190c4784b6f4 | 104 | M1_in2 = 0; |
cudaChen | 0:190c4784b6f4 | 105 | } else { |
cudaChen | 0:190c4784b6f4 | 106 | // right wheel go backward |
cudaChen | 0:190c4784b6f4 | 107 | M1_in1 = 0; |
cudaChen | 0:190c4784b6f4 | 108 | M1_in2 = 1; |
cudaChen | 0:190c4784b6f4 | 109 | } |
cudaChen | 0:190c4784b6f4 | 110 | //M1_enable.write_u16(_speed); // 驅動馬達 右輪 |
cudaChen | 0:190c4784b6f4 | 111 | M1_enable.write(1); |
cudaChen | 0:190c4784b6f4 | 112 | |
cudaChen | 0:190c4784b6f4 | 113 | } else if (m == MOTOR_M2) { |
cudaChen | 0:190c4784b6f4 | 114 | // 設定方向 |
cudaChen | 0:190c4784b6f4 | 115 | if (speed == PWR_STOP) { |
cudaChen | 0:190c4784b6f4 | 116 | M2_in3 = 0; |
cudaChen | 0:190c4784b6f4 | 117 | M2_in4 = 0; |
cudaChen | 0:190c4784b6f4 | 118 | } else if (dir == DIR_FORWARD) { |
cudaChen | 0:190c4784b6f4 | 119 | //左輪前進 |
cudaChen | 0:190c4784b6f4 | 120 | M2_in3 = 1; |
cudaChen | 0:190c4784b6f4 | 121 | M2_in4 = 0; |
cudaChen | 0:190c4784b6f4 | 122 | } else { |
cudaChen | 0:190c4784b6f4 | 123 | //左輪倒轉 |
cudaChen | 0:190c4784b6f4 | 124 | M2_in3 = 0; |
cudaChen | 0:190c4784b6f4 | 125 | M2_in4 = 1; |
cudaChen | 0:190c4784b6f4 | 126 | } |
cudaChen | 0:190c4784b6f4 | 127 | //M2_enable.write_u16(_speed); // 驅動馬達 左輪 |
cudaChen | 0:190c4784b6f4 | 128 | M2_enable.write(1); |
cudaChen | 0:190c4784b6f4 | 129 | } else { |
cudaChen | 0:190c4784b6f4 | 130 | //M1_enable.write_u16(PWR_STOP); // right wheel |
cudaChen | 0:190c4784b6f4 | 131 | //M2_enable.write_u16(PWR_STOP); // left wheel |
cudaChen | 0:190c4784b6f4 | 132 | M1_enable.write(0); |
cudaChen | 0:190c4784b6f4 | 133 | M2_enable.write(0); |
cudaChen | 0:190c4784b6f4 | 134 | } |
cudaChen | 0:190c4784b6f4 | 135 | } |
cudaChen | 0:190c4784b6f4 | 136 | |
cudaChen | 5:25faf509ee9b | 137 | void driveMotor(bool left, bool middle, bool right) |
cudaChen | 5:25faf509ee9b | 138 | { |
cudaChen | 6:f34538eea724 | 139 | int status = left * 4 + middle * 2 + right; |
cudaChen | 6:f34538eea724 | 140 | switch(status) { |
cudaChen | 6:f34538eea724 | 141 | case 7: // go straight |
cudaChen | 6:f34538eea724 | 142 | pc.printf("7. go straight\r\n"); |
cudaChen | 6:f34538eea724 | 143 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 144 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 145 | break; |
cudaChen | 6:f34538eea724 | 146 | case 6: // turn left |
cudaChen | 6:f34538eea724 | 147 | pc.printf("6. turn left\r\n"); |
cudaChen | 6:f34538eea724 | 148 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 149 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 150 | break; |
cudaChen | 6:f34538eea724 | 151 | case 5: // go straight |
cudaChen | 6:f34538eea724 | 152 | pc.printf("5. go straight\r\n"); |
cudaChen | 6:f34538eea724 | 153 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 154 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 155 | break; |
cudaChen | 6:f34538eea724 | 156 | case 4: // turn left |
cudaChen | 6:f34538eea724 | 157 | pc.printf("4. turn left\r\n"); |
cudaChen | 6:f34538eea724 | 158 | DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 159 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 160 | break; |
cudaChen | 6:f34538eea724 | 161 | case 3: // turn right |
cudaChen | 6:f34538eea724 | 162 | pc.printf("3. turn right\r\n"); |
cudaChen | 6:f34538eea724 | 163 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 164 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 165 | break; |
cudaChen | 6:f34538eea724 | 166 | case 2: // go straight |
cudaChen | 6:f34538eea724 | 167 | pc.printf("2. go straight\r\n"); |
cudaChen | 6:f34538eea724 | 168 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 169 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 170 | break; |
cudaChen | 6:f34538eea724 | 171 | case 1: // turn right |
cudaChen | 6:f34538eea724 | 172 | pc.printf("1. turn right\r\n"); |
cudaChen | 6:f34538eea724 | 173 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 174 | DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 175 | break; |
cudaChen | 6:f34538eea724 | 176 | case 0: // go straight |
cudaChen | 6:f34538eea724 | 177 | pc.printf("0. go straight\r\n"); |
cudaChen | 6:f34538eea724 | 178 | DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 179 | DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD); |
cudaChen | 6:f34538eea724 | 180 | break; |
cudaChen | 6:f34538eea724 | 181 | default: |
cudaChen | 6:f34538eea724 | 182 | pc.printf("invalid\r\n"); |
cudaChen | 6:f34538eea724 | 183 | break; |
cudaChen | 6:f34538eea724 | 184 | } |
cudaChen | 5:25faf509ee9b | 185 | } |
cudaChen | 5:25faf509ee9b | 186 |