Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp@13:87cd0ae37e06, 2018-07-01 (annotated)
- Committer:
- cudaChen
- Date:
- Sun Jul 01 07:58:20 2018 +0000
- Revision:
- 13:87cd0ae37e06
- Parent:
- 12:e95ed962be7a
- Child:
- 15:1d440beb24d3
[add] create PID control feature (unfinished)
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 | 7:4edd049209a6 | 5 | // trigger button and triggered LED |
cudaChen | 0:190c4784b6f4 | 6 | DigitalOut led1(LED1); |
cudaChen | 7:4edd049209a6 | 7 | DigitalIn pb(PC_13); |
cudaChen | 7:4edd049209a6 | 8 | int lastButtonState = 0; |
cudaChen | 7:4edd049209a6 | 9 | bool ledState = false; |
cudaChen | 0:190c4784b6f4 | 10 | |
cudaChen | 13:87cd0ae37e06 | 11 | // PID control params |
cudaChen | 13:87cd0ae37e06 | 12 | int values = 0; |
cudaChen | 13:87cd0ae37e06 | 13 | float Kp = 0.2, Ki = 0.0001, Kd = 1; |
cudaChen | 13:87cd0ae37e06 | 14 | |
cudaChen | 0:190c4784b6f4 | 15 | // main() runs in its own thread in the OS |
cudaChen | 0:190c4784b6f4 | 16 | int main() |
cudaChen | 0:190c4784b6f4 | 17 | { |
cudaChen | 13:87cd0ae37e06 | 18 | /*bool left = false; |
cudaChen | 0:190c4784b6f4 | 19 | bool middle = false; |
cudaChen | 13:87cd0ae37e06 | 20 | bool right = false;*/ |
cudaChen | 4:76b9213714cc | 21 | |
cudaChen | 2:e0a553b64315 | 22 | // here I use 500 as threshold |
cudaChen | 2:e0a553b64315 | 23 | int threshold = 500; |
cudaChen | 0:190c4784b6f4 | 24 | |
cudaChen | 10:a14380381d86 | 25 | // set two motors to stop |
cudaChen | 10:a14380381d86 | 26 | init(); |
cudaChen | 10:a14380381d86 | 27 | |
cudaChen | 4:76b9213714cc | 28 | while (true) { |
cudaChen | 7:4edd049209a6 | 29 | int reading1 = pb.read(); |
cudaChen | 10:a14380381d86 | 30 | |
cudaChen | 7:4edd049209a6 | 31 | if(reading1 != lastButtonState) { |
cudaChen | 7:4edd049209a6 | 32 | wait_ms(20); |
cudaChen | 10:a14380381d86 | 33 | |
cudaChen | 7:4edd049209a6 | 34 | int reading2 = pb.read(); |
cudaChen | 10:a14380381d86 | 35 | |
cudaChen | 7:4edd049209a6 | 36 | if(reading2 == reading1) { |
cudaChen | 7:4edd049209a6 | 37 | lastButtonState = reading2; |
cudaChen | 7:4edd049209a6 | 38 | } |
cudaChen | 10:a14380381d86 | 39 | |
cudaChen | 7:4edd049209a6 | 40 | if(lastButtonState == 1) { |
cudaChen | 7:4edd049209a6 | 41 | ledState = !ledState; |
cudaChen | 7:4edd049209a6 | 42 | } |
cudaChen | 7:4edd049209a6 | 43 | } |
cudaChen | 10:a14380381d86 | 44 | |
cudaChen | 7:4edd049209a6 | 45 | led1.write(ledState); |
cudaChen | 10:a14380381d86 | 46 | |
cudaChen | 7:4edd049209a6 | 47 | if(ledState) { |
cudaChen | 10:a14380381d86 | 48 | // not on track: > 500 |
cudaChen | 10:a14380381d86 | 49 | // on track (black): < 500 |
cudaChen | 13:87cd0ae37e06 | 50 | //readIR(&left, &middle, &right, threshold); |
cudaChen | 13:87cd0ae37e06 | 51 | values = readIRValues(); // read IR values |
cudaChen | 0:190c4784b6f4 | 52 | |
cudaChen | 13:87cd0ae37e06 | 53 | //driveMotor(left, middle, right); |
cudaChen | 13:87cd0ae37e06 | 54 | driveMotorPID(values, Kp, Ki, Kd); |
cudaChen | 7:4edd049209a6 | 55 | } |
cudaChen | 0:190c4784b6f4 | 56 | } |
cudaChen | 0:190c4784b6f4 | 57 | } |