Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp@15:1d440beb24d3, 2018-07-05 (annotated)
- Committer:
- cudaChen
- Date:
- Thu Jul 05 09:33:33 2018 +0000
- Revision:
- 15:1d440beb24d3
- Parent:
- 13:87cd0ae37e06
- Child:
- 16:3202c73881a3
[add] add statements which user can define the pins they want to use
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 | 12:e95ed962be7a | 3 | #include "autocar.h" |
cudaChen | 12:e95ed962be7a | 4 | |
cudaChen | 15:1d440beb24d3 | 5 | DigitalOut led1(LED1); // LED indicating the car is running |
cudaChen | 15:1d440beb24d3 | 6 | DigitalIn pb(PC_13); // triggter button |
cudaChen | 7:4edd049209a6 | 7 | int lastButtonState = 0; |
cudaChen | 7:4edd049209a6 | 8 | bool ledState = false; |
cudaChen | 0:190c4784b6f4 | 9 | |
cudaChen | 15:1d440beb24d3 | 10 | // output used for controlling motors |
cudaChen | 15:1d440beb24d3 | 11 | DigitalOut M1_enable(D6); |
cudaChen | 15:1d440beb24d3 | 12 | DigitalOut M2_enable(D5); |
cudaChen | 15:1d440beb24d3 | 13 | DigitalOut M1_in1(D3); // connected to left wheel |
cudaChen | 15:1d440beb24d3 | 14 | DigitalOut M1_in2(D2); // connected to left wheel |
cudaChen | 15:1d440beb24d3 | 15 | DigitalOut M2_in3(D1); // connected to right wheel |
cudaChen | 15:1d440beb24d3 | 16 | DigitalOut M2_in4(D0); // connected to right wheel |
cudaChen | 15:1d440beb24d3 | 17 | |
cudaChen | 15:1d440beb24d3 | 18 | // input used for IR sensors |
cudaChen | 15:1d440beb24d3 | 19 | extern AnalogIn leftIR(A1); // left sensor |
cudaChen | 15:1d440beb24d3 | 20 | extern AnalogIn middleIR(A3); // middle sensor |
cudaChen | 15:1d440beb24d3 | 21 | extern AnalogIn rightIR(A5); // right sensor |
cudaChen | 15:1d440beb24d3 | 22 | |
cudaChen | 13:87cd0ae37e06 | 23 | // PID control params |
cudaChen | 13:87cd0ae37e06 | 24 | int values = 0; |
cudaChen | 13:87cd0ae37e06 | 25 | float Kp = 0.2, Ki = 0.0001, Kd = 1; |
cudaChen | 13:87cd0ae37e06 | 26 | |
cudaChen | 0:190c4784b6f4 | 27 | // main() runs in its own thread in the OS |
cudaChen | 0:190c4784b6f4 | 28 | int main() |
cudaChen | 0:190c4784b6f4 | 29 | { |
cudaChen | 15:1d440beb24d3 | 30 | bool left = false; |
cudaChen | 0:190c4784b6f4 | 31 | bool middle = false; |
cudaChen | 15:1d440beb24d3 | 32 | bool right = false; |
cudaChen | 4:76b9213714cc | 33 | |
cudaChen | 2:e0a553b64315 | 34 | // here I use 500 as threshold |
cudaChen | 2:e0a553b64315 | 35 | int threshold = 500; |
cudaChen | 0:190c4784b6f4 | 36 | |
cudaChen | 10:a14380381d86 | 37 | // set two motors to stop |
cudaChen | 10:a14380381d86 | 38 | init(); |
cudaChen | 10:a14380381d86 | 39 | |
cudaChen | 4:76b9213714cc | 40 | while (true) { |
cudaChen | 7:4edd049209a6 | 41 | int reading1 = pb.read(); |
cudaChen | 10:a14380381d86 | 42 | |
cudaChen | 7:4edd049209a6 | 43 | if(reading1 != lastButtonState) { |
cudaChen | 7:4edd049209a6 | 44 | wait_ms(20); |
cudaChen | 10:a14380381d86 | 45 | |
cudaChen | 7:4edd049209a6 | 46 | int reading2 = pb.read(); |
cudaChen | 10:a14380381d86 | 47 | |
cudaChen | 7:4edd049209a6 | 48 | if(reading2 == reading1) { |
cudaChen | 7:4edd049209a6 | 49 | lastButtonState = reading2; |
cudaChen | 7:4edd049209a6 | 50 | } |
cudaChen | 10:a14380381d86 | 51 | |
cudaChen | 7:4edd049209a6 | 52 | if(lastButtonState == 1) { |
cudaChen | 7:4edd049209a6 | 53 | ledState = !ledState; |
cudaChen | 7:4edd049209a6 | 54 | } |
cudaChen | 7:4edd049209a6 | 55 | } |
cudaChen | 10:a14380381d86 | 56 | |
cudaChen | 7:4edd049209a6 | 57 | led1.write(ledState); |
cudaChen | 10:a14380381d86 | 58 | |
cudaChen | 7:4edd049209a6 | 59 | if(ledState) { |
cudaChen | 10:a14380381d86 | 60 | // not on track: > 500 |
cudaChen | 10:a14380381d86 | 61 | // on track (black): < 500 |
cudaChen | 15:1d440beb24d3 | 62 | readIR(&left, &middle, &right, threshold); |
cudaChen | 15:1d440beb24d3 | 63 | //values = readIRValues(); // read IR values |
cudaChen | 0:190c4784b6f4 | 64 | |
cudaChen | 15:1d440beb24d3 | 65 | driveMotor(left, middle, right); |
cudaChen | 15:1d440beb24d3 | 66 | //driveMotorPID(values, Kp, Ki, Kd); |
cudaChen | 7:4edd049209a6 | 67 | } |
cudaChen | 0:190c4784b6f4 | 68 | } |
cudaChen | 0:190c4784b6f4 | 69 | } |