Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

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?

UserRevisionLine numberNew 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 }