File content as of revision 21:093c8525349a:

#include "mbed.h"

#include "Ping.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

// input used for ultrasonic sensors
Ping ultrasonic(D13);

// PID control params
int values = 0;
float Kp = 0.7, Ki = 0, Kd = 0;

// main() runs in its own thread in the OS
int main()
{
    bool left = false;
    bool middle = false;
    bool right = false;
    bool hasObstacle = false;

    int threshold = 500; // here I use 500 as threshold of IR sensor
    int range = 30; // range of obstacle (cm)

    // 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);
            //readSensor(&left, &middle, &right, &hasObstacle, threshold, range);
            //values = readIRValues(); // read IR values
            readSensorValues(&values, &hasObstacle, range);

            //driveMotor(left, middle, right);
            //driveMotor(left, middle, right, hasObstacle);
            //driveMotorPID(values, Kp, Ki, Kd);
            run(values, Kp, Ki, Kd, hasObstacle);
        }
    }
}