Lu-Hsuan Chen
/
autocar_3sensor_mbed
An auto car with 3 IR sensors.
main.cpp
- Committer:
- cudaChen
- Date:
- 2018-07-06
- Revision:
- 16:3202c73881a3
- Parent:
- 15:1d440beb24d3
- Child:
- 18:d7509436e9ef
File content as of revision 16:3202c73881a3:
#include "mbed.h" #include "autocar.h" DigitalOut led1(LED1); // LED indicating the car is running DigitalIn pb(PC_13); // triggter button int lastButtonState = 0; bool ledState = false; // output used for controlling motors DigitalOut M1_enable(D6); DigitalOut M2_enable(D5); DigitalOut M1_in1(D3); // connected to left motor (IN1) DigitalOut M1_in2(D2); // connected to left motor (IN2) DigitalOut M2_in3(D1); // connected to right motor (IN3) DigitalOut M2_in4(D0); // connected to right motor (IN4) // input used for IR sensors AnalogIn leftIR(A1); // left sensor AnalogIn middleIR(A3); // middle sensor AnalogIn rightIR(A5); // right sensor // PID control params int values = 0; float Kp = 0.2, Ki = 0.0001, Kd = 1; // main() runs in its own thread in the OS int main() { bool left = false; bool middle = false; bool right = false; // here I use 500 as threshold int threshold = 500; // set two motors to stop init(); while (true) { int reading1 = pb.read(); if(reading1 != lastButtonState) { wait_ms(20); int reading2 = pb.read(); if(reading2 == reading1) { lastButtonState = reading2; } if(lastButtonState == 1) { ledState = !ledState; } } led1.write(ledState); if(ledState) { // not on track: > 500 // on track (black): < 500 readIR(&left, &middle, &right, threshold); //values = readIRValues(); // read IR values driveMotor(left, middle, right); //driveMotorPID(values, Kp, Ki, Kd); } } }