File content as of revision 101:efa2315d0312:

#include "mbed.h"
//#include "motor.h"
#include "ReceiverIR.h"
#include "Adafruit_SSD1306.h"   // OLED
#define NUM_SENSORS 5
 
//Motor motor(D6, D5, PA_0, PA_1, PB_0, PA_4, 0.3);
ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4);
SPI spi(D11, D12, D13);
DigitalOut spi_cs(D10, 1);
Serial pc(SERIAL_TX, SERIAL_RX, 115200);
I2C i2c(D14, D15);                                      // D14, D15
Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);    // D9, (D8: D/C - data/command change)
DigitalOut DataComm(D8);

Serial blue(PA_11, PA_12, 9600);
 
Thread TR_thread;
Thread remote_thread;
Thread oled_thread;
 
unsigned long avg = 0;
unsigned int sum = 0;
 
unsigned int sensor_values[NUM_SENSORS];
unsigned int max_sensor_values[NUM_SENSORS];
unsigned int min_sensor_values[NUM_SENSORS];
 
unsigned int *calibratedMin;
unsigned int *calibratedMax;
 
int value;
float bat;
int on_line = 0;
static int pos = 0;
unsigned int last_proportional = 0;
long integral = 0;
 
volatile int flag = 0;
volatile int start = 0;
 
int ch;
 
 
 
int readLine(unsigned int *sensor_values);
void tr_ready(void);
void calibration(void);
void init(void);
 
void tracing_pid(void);
void set_ir_val(void);
void get_pos(void);
int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
void remote_ctrl(void);
void oled_disp(void);
 
int main()
{
    pc.printf("start\r\n");
    spi.format(16, 0);
    spi.frequency(2000000);
 
    init();
 
    oled_thread.start(&oled_disp);
    remote_thread.start(&remote_ctrl);
    while(!motor.cal_start);
 
    calibration();
 
    while(!flag);
 
    TR_thread.start(&tr_ready);
 
 
    while(!start);
 
    pc.printf("motor forward\r\n");
    motor.forward();
 
    while (1);
}
 
void remote_ctrl(void){
 
    while(1){
    uint8_t buf1[32];
    uint8_t buf2[32];
    int bitlength1;
    RemoteIR::Format format;
 
    memset(buf1, 0x00, sizeof(buf1));
    memset(buf2, 0x00, sizeof(buf2));
 
        bitlength1 = receive(&format, buf1, sizeof(buf1));
        if (bitlength1 < 0) {
            continue;
        }
 
    }
}
 
void init(void){
    motor.cal_start = 0;
    motor.cal_stop = 0; 
    
    motor.kp = 10;
    motor.kd = 1;
    motor.ki = 5000;
    motor.max = 150;
 
    DataComm = 0;
 
    calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
    calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
 
    for(int i=0; i < NUM_SENSORS; i++)
    {
      calibratedMin[i] = 1023;
      calibratedMax[i] = 0;
    }
}
 
void oled_disp(void){
    myOled.begin();
    while(1){
        myOled.clearDisplay();
        //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
        myOled.printf("%.0f %.1f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
        myOled.display();
    }
}
 
 
int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
    int cnt = 0;
    while (motor.getState() != ReceiverIR::Received) {
        cnt++;
        if (timeout < cnt) {
            return -1;
        }
    }
    return motor.getData(format, buf, bufsiz * 8);
}
 
 
void read_ir(void){
    ch = 0;
 
    spi_cs = 0;
    wait_us(2);
    value = spi.write(ch << 12);
    spi_cs = 1;
 
    wait_us(21);
 
    for(int i = 0; i < 5; i++){
 
        ch += 1;
 
        spi_cs = 0;
        wait_us(2);
        value = spi.write(ch << 12);
        spi_cs = 1;
 
        wait_us(21);
 
        value = value >> 6;
        value = value * 3300 / 0x3FF;
 
        sensor_values[i] = value;
    }
}
 
void calibration(void){
    pc.printf("calibration start \r\n");
    
    int i = 0;
    while(!motor.cal_stop){    
        read_ir();
        for(int j = 0; j < NUM_SENSORS; j++){
            if(i == 0 || max_sensor_values[j] < sensor_values[j]){
                max_sensor_values[j] = sensor_values[j];
            }
 
            if(i == 0 || min_sensor_values[j] > sensor_values[j]){
                min_sensor_values[j] = sensor_values[j];
            }
        }
        i = 1;
    }
 
    for(int i = 0; i < NUM_SENSORS; i++){
        if(min_sensor_values[i] < calibratedMin[i])
            calibratedMin[i] = min_sensor_values[i];
        if(max_sensor_values[i] > calibratedMax[i])
            calibratedMax[i] = max_sensor_values[i];
    }
    
    pc.printf("calibration complete\r\n");
    flag = 1;
}
 
void tr_ready(void){
 
    while(1){
        // read current IR 1 ~ IR 5
        read_ir();
 
        // set range under 1000
        set_ir_val();
 
        // get current position
        get_pos();
 
        tracing_pid();
        
        
    }
}
 
void set_ir_val(){
    for(int i = 0; i < NUM_SENSORS; i++)
    {
        unsigned int denominator;
 
        denominator = calibratedMax[i] - calibratedMin[i];
 
        signed int x = 0;
        if(denominator != 0)
            x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
        if(x < 0)   x = 0;
        else if(x > 1000)   x = 1000;
 
        sensor_values[i] = x;
        
   }
   
   // finish line
   if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
        motor.stop();
        wait(5);        
    }
        
   for(int i = 0; i < NUM_SENSORS; i++){
 
        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
    }
    
    pc.printf("\r\n");
}
 
void get_pos(){
    on_line = 0;
    avg = 0;
    sum = 0;
 
    for(int i = 0; i < NUM_SENSORS; i++){
        int val = sensor_values[i];
 
        // determine "on_line" or "out_line"
        if(val < 500){
            on_line = 1;
        }
 
        // under
        if(val > 5){
            avg += (long)(val) * (i * 1000);
            sum += val;
        }
    }
 
    // out_line
    if(!on_line){
        if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
            pos = 0;                             // last_vlaue = 0
        }
        else{                                    // right -> out-line (over 2000)
             pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
        }
    }
    // on_line
    else{
        pos = avg / sum;
    }
 
    start = 1;
}
 
void tracing_pid(void){
 
    
    int proportional = pos - 2000;
    
    int derivative = proportional - last_proportional;
    integral += proportional;
 
    last_proportional  = proportional;
 
    int power_difference = proportional / motor.kp + integral / motor.ki + derivative * motor.kd;
    //int power_difference = proportional / motor.kp;
    //int power_difference = proportional/motor.kp + derivative * motor.kd; 
    const float std = 600.0;
 
    if(power_difference > motor.max)
        power_difference = motor.max;
 
    if(power_difference < -motor.max)
        power_difference = -motor.max;
    
    // bot position: right
    if(power_difference > 15){ //0.1 + p_d/std
        motor.speed_r((motor.max + power_difference)/std);
        motor.speed_l((motor.max - power_difference)/std);
    }
    // bot position: left
    else if(power_difference < -15){
        motor.speed_r((motor.max + power_difference)/std);
        motor.speed_l((motor.max - power_difference)/std);
    }
    else{ //online
        motor.speed_l(motor.init_sp_l);
        motor.speed_r(motor.init_sp_r);
    }
 
    // pc.printf("proportional: %d\r\n", proportional);
     //pc.printf("(max + power_difference): %d\r\n", motor.max + power_difference);
    // pc.printf("derivative: %d\r\n", derivative);
    // pc.printf("integral: %d\r\n",integral);
}