Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

main.cpp@16:3202c73881a3, 2018-07-06 (annotated)

Committer:
cudaChen
Date:
Fri Jul 06 05:18:18 2018 +0000
Revision:
16:3202c73881a3
Parent:
15:1d440beb24d3
Child:
18:d7509436e9ef
[fix] fix typo

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 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 16:3202c73881a3 13 DigitalOut M1_in1(D3); // connected to left motor (IN1)
cudaChen 16:3202c73881a3 14 DigitalOut M1_in2(D2); // connected to left motor (IN2)
cudaChen 16:3202c73881a3 15 DigitalOut M2_in3(D1); // connected to right motor (IN3)
cudaChen 16:3202c73881a3 16 DigitalOut M2_in4(D0); // connected to right motor (IN4)
cudaChen 15:1d440beb24d3 17
cudaChen 15:1d440beb24d3 18 // input used for IR sensors
cudaChen 16:3202c73881a3 19 AnalogIn leftIR(A1); // left sensor
cudaChen 16:3202c73881a3 20 AnalogIn middleIR(A3); // middle sensor
cudaChen 16:3202c73881a3 21 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 }