File content as of revision 4:982dcc2390a2:

#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;

// 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() {
    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() {
    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);
}