File content as of revision 97:bad45e2dc7e1:

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

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(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.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;
        }
        
        //display_status("RECV", bitlength1);
        //display_data(buf1, bitlength1);
        //display_format(format);  

    }  
}

void init(void){
    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.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");
    
    for(int i = 0; i < 10; i++){
        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];
            }
        }
        wait(0.5);
    }
    
    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();
        
//        pc.printf("pos = %d\r\n", pos);
        // start tracing_pid_pid
        //tracing_pid();
        tracing();      
//        pc.printf("pos = %d\r\n", pos);
//        pc.printf("on_line = %d\r\n", on_line);
                        
//        wait(1);
    }             
}

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;
   }
//   for(int i = 0; i <NUM_SENSORS; i++){
//        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
//    }
}

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 < 800){
            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;        
    }
    pc.printf("position: %d\r\n", pos);
    start = 1;
}

void tracing(){
    // totally right
    if(pos == 4000){
        motor.speedup_r(3);
    }
    // detect IR1 on line
    else if(pos >= 2300){
        motor.speedup_r(3);    
    }
    // detect IR2 on line
    else if(pos >= 2100){
        motor.speedup_r(1);
    }
    // detect IR3 on line
    else if(pos >= 1900){
        motor.speed_r(0.09);
        motor.speed_l(0.085);
    }
    // detect IR4 on line
    else if(pos >= 1700){
        motor.speedup_l(1);    
    }
    // detect IR5 on line
    else if(pos >= 1200){
        motor.speedup_l(3);    
    }
    // totally left
    else if(pos == 0){
        motor.speedup_l(3);            
    }
    else {
        pc.printf("error pos: %d\r\n", pos);
        pc.printf("error\r\n");
    }
    wait(0.001);

}

void tracing_pid(void){

    int proportional = pos - 2000;
    
    int derivative = proportional - last_proportional;
    integral += proportional;
    
    last_proportional  = proportional;
    
    int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
//    int power_difference = proportional / ;
    
    const int max = 150;
    
    if(power_difference > max)
        power_difference = max;
        
    if(power_difference < -max)
        power_difference = -max;
    
    if(power_difference < 0){
        motor.speed_l((max + power_difference)/255);
        motor.speed_r(max/255);
    }
    else{
        motor.speed_l(max/255);
        motor.speed_r((max - power_difference)/255);
    }
    
    pc.printf("proportional: %d\r\n", proportional);
    pc.printf("power_difference: %d\r\n", power_difference);
    pc.printf("derivative: %d\r\n", derivative);    
    pc.printf("integral: %d\r\n",integral); 
}