File content as of revision 5:3caf5b6ed35a:

#include "mbed.h"

#include "motorcar.h"
#include "Ping.h"

// output used for controlling motors
AnalogOut M1_enable(PA_4); // right motor enable signal
AnalogOut M2_enable(PA_5); // left motor enable signal
DigitalOut M1_in1(D3); // connected to right motor (IN1)
DigitalOut M1_in2(D2); // connected to right motor (IN2)
DigitalOut M2_in3(D1); // connected to left motor (IN3)
DigitalOut M2_in4(D0); // connected to left motor (IN4)

// input used for IR sensors
AnalogIn leftIR(A1); // left sensor
AnalogIn middleIR(A3); // middle sensor
AnalogIn rightIR(A5); // right sensor

// used for storing returned IR sensors' value
int left, middle, right;

// used for ultra sonic sensor
AnalogIn Ping ultraSonic(D12);
int distance;
bool hasObstacle = false;
int range = 20;

// PID factors
int interror = 0;
int olderror = 0;
int values = 0;
float Kp = 1, Ki = 0, Kd = 0;
/* Though STM32 only has 12-bit DAC,
 * mbed still cam map the value by
 * using write_u16()
 */
int limit = 65535; // 2^16 - 1

// used for output message via serial for ease of debugging
//Serial pc(SERIAL_TX, SERIAL_RX);

void readSensorValue() {
    ultraSonic.Send();
    wait_ms(30);
    distance = ultraSonic.Read_cm();
    hasObstacle = distance < range ? true : false;
    
    left = leftIR.read_u16();
    middle = middleIR.read_u16();
    right = rightIR.read_u16();
    
    values = left * (-1) + middle * 0 + right * 1;

    //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
    //wait(1);
    //pc.printf("IR values: %d\r\n", ret);
}

void runPID() {
    // if there is an obstacle
    if(hasObstacle) {
        speed(0, 0);
        wait_ms(200);
        speed(0, 200);
        wait_ms(200);
        speed(0, 0);
        wait_ms(200);
        speed(200, 200);
        wait_ms(800);
        speed(0, 0);
        wait_ms(200);
        speed(200, 0);
        wait_ms(200);
        speed(0, 0);
        wait_ms(200);
        return;
    }
    
    int error = values;
    interror += error;
    int lasterror = error - olderror;
    olderror = error;
    int power = error * Kp + interror * Ki + lasterror * Kd;
    
    // limit the output of PID value
    if(power > limit) {
        power = limit;
    } 
    if(power < -limit) {
        power = -limit;
    }
    
    // PID works here
    if(power > 0) {
        speed(limit, limit - power);
    } else {
        speed(limit + power, limit);
    }
}

void init() {
    speed(0, 0);
}

void speed(int left, int right) {
    // control right wheel
    if(right < 0) { 
        // go backward
        right = -right;
        M1_in1 = 0;
        M1_in2 = 1;
    } else if(right == 0) {
        // stop
        M1_in1 = 0;
        M1_in2 = 0;
    } else {
        // go forward
        M1_in1 = 1;
        M1_in2 = 0;
    }        
    M1_enable.write_u16(right);
    
    // control left wheel
    if(left < 0) {
        // go backward
        left = -left;
        M2_in3 = 0;
        M2_in4 = 1;
    } else if(left == 0) {
        // stop
        M2_in3 = 0;
        M2_in4 = 0;
    } else {
        // go forward
        M2_in3 = 1;
        M2_in4 = 0;
    }
    M2_enable.write_u16(left);
}