Lu-Hsuan Chen / Mbed OS autocar_3sensor_mbed

An auto car with 3 IR sensors.

Dependencies:   Ping

main.cpp@21:093c8525349a, 2018-07-19 (annotated)

Committer:
cudaChen
Date:
Thu Jul 19 07:41:10 2018 +0000
Revision:
21:093c8525349a
Parent:
20:cc12f841a06e 
[change] merge "PID control" and "obstacle avoidance" feature

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cudaChen 0:190c4784b6f4 1 #include "mbed.h"
cudaChen 0:190c4784b6f4 2
cudaChen 18:d7509436e9ef 3 #include "Ping.h"
cudaChen 12:e95ed962be7a 4 #include "autocar.h"
cudaChen 12:e95ed962be7a 5
cudaChen 15:1d440beb24d3 6 DigitalOut led1(LED1); // LED indicating the car is running
cudaChen 15:1d440beb24d3 7 DigitalIn pb(PC_13); // triggter button
cudaChen 7:4edd049209a6 8 int lastButtonState = 0;
cudaChen 7:4edd049209a6 9 bool ledState = false;
cudaChen 0:190c4784b6f4 10
cudaChen 15:1d440beb24d3 11 // output used for controlling motors
cudaChen 15:1d440beb24d3 12 DigitalOut M1_enable(D6);
cudaChen 15:1d440beb24d3 13 DigitalOut M2_enable(D5);
cudaChen 16:3202c73881a3 14 DigitalOut M1_in1(D3); // connected to left motor (IN1)
cudaChen 16:3202c73881a3 15 DigitalOut M1_in2(D2); // connected to left motor (IN2)
cudaChen 16:3202c73881a3 16 DigitalOut M2_in3(D1); // connected to right motor (IN3)
cudaChen 16:3202c73881a3 17 DigitalOut M2_in4(D0); // connected to right motor (IN4)
cudaChen 15:1d440beb24d3 18
cudaChen 15:1d440beb24d3 19 // input used for IR sensors
cudaChen 16:3202c73881a3 20 AnalogIn leftIR(A1); // left sensor
cudaChen 16:3202c73881a3 21 AnalogIn middleIR(A3); // middle sensor
cudaChen 16:3202c73881a3 22 AnalogIn rightIR(A5); // right sensor
cudaChen 15:1d440beb24d3 23
cudaChen 18:d7509436e9ef 24 // input used for ultrasonic sensors
cudaChen 18:d7509436e9ef 25 Ping ultrasonic(D13);
cudaChen 18:d7509436e9ef 26
cudaChen 13:87cd0ae37e06 27 // PID control params
cudaChen 19:d06f5a3ed0bc 28 int values = 0;
cudaChen 20:cc12f841a06e 29 float Kp = 0.7, Ki = 0, Kd = 0;
cudaChen 13:87cd0ae37e06 30
cudaChen 0:190c4784b6f4 31 // main() runs in its own thread in the OS
cudaChen 0:190c4784b6f4 32 int main()
cudaChen 0:190c4784b6f4 33 {
cudaChen 15:1d440beb24d3 34 bool left = false;
cudaChen 0:190c4784b6f4 35 bool middle = false;
cudaChen 15:1d440beb24d3 36 bool right = false;
cudaChen 18:d7509436e9ef 37 bool hasObstacle = false;
cudaChen 4:76b9213714cc 38
cudaChen 18:d7509436e9ef 39 int threshold = 500; // here I use 500 as threshold of IR sensor
cudaChen 18:d7509436e9ef 40 int range = 30; // range of obstacle (cm)
cudaChen 0:190c4784b6f4 41
cudaChen 10:a14380381d86 42 // set two motors to stop
cudaChen 10:a14380381d86 43 init();
cudaChen 10:a14380381d86 44
cudaChen 4:76b9213714cc 45 while (true) {
cudaChen 7:4edd049209a6 46 int reading1 = pb.read();
cudaChen 10:a14380381d86 47
cudaChen 7:4edd049209a6 48 if(reading1 != lastButtonState) {
cudaChen 7:4edd049209a6 49 wait_ms(20);
cudaChen 10:a14380381d86 50
cudaChen 7:4edd049209a6 51 int reading2 = pb.read();
cudaChen 10:a14380381d86 52
cudaChen 7:4edd049209a6 53 if(reading2 == reading1) {
cudaChen 7:4edd049209a6 54 lastButtonState = reading2;
cudaChen 7:4edd049209a6 55 }
cudaChen 10:a14380381d86 56
cudaChen 7:4edd049209a6 57 if(lastButtonState == 1) {
cudaChen 7:4edd049209a6 58 ledState = !ledState;
cudaChen 7:4edd049209a6 59 }
cudaChen 7:4edd049209a6 60 }
cudaChen 10:a14380381d86 61
cudaChen 7:4edd049209a6 62 led1.write(ledState);
cudaChen 10:a14380381d86 63
cudaChen 7:4edd049209a6 64 if(ledState) {
cudaChen 10:a14380381d86 65 // not on track: > 500
cudaChen 10:a14380381d86 66 // on track (black): < 500
cudaChen 18:d7509436e9ef 67 //readIR(&left, &middle, &right, threshold);
cudaChen 20:cc12f841a06e 68 //readSensor(&left, &middle, &right, &hasObstacle, threshold, range);
cudaChen 21:093c8525349a 69 //values = readIRValues(); // read IR values
cudaChen 21:093c8525349a 70 readSensorValues(&values, &hasObstacle, range);
cudaChen 0:190c4784b6f4 71
cudaChen 18:d7509436e9ef 72 //driveMotor(left, middle, right);
cudaChen 20:cc12f841a06e 73 //driveMotor(left, middle, right, hasObstacle);
cudaChen 21:093c8525349a 74 //driveMotorPID(values, Kp, Ki, Kd);
cudaChen 21:093c8525349a 75 run(values, Kp, Ki, Kd, hasObstacle);
cudaChen 7:4edd049209a6 76 }
cudaChen 0:190c4784b6f4 77 }
cudaChen 0:190c4784b6f4 78 }