File content as of revision 112:744fef64b8ba:

#include "mbed.h"
//#include "motor.h"
#include "ReceiverIR.h"
#include "Adafruit_SSD1306.h"   // OLED
#include "WS2812.h"             // botom led
#include "PixelArray.h"         // bottom led


#define NUM_SENSORS 5
#define PCF8574_ADDR 0x20 //0b0010_0000

#define WS2812_BUF 10
#define NUM_COLORS 6
#define NUM_LEDS_PER_COLOR 4

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);
RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
I2C i2c(I2C_SDA, I2C_SCL);                                    // D14, D15
Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);    // D9, (D8: D/C - data/command change)
DigitalOut DataComm(D8);
 
PixelArray px(WS2812_BUF);
WS2812 ws(D7, WS2812_BUF, 6, 17, 9, 14);
 
Thread TR_thread;
Thread remote_thread;
Thread oled_thread;


// Timer
Timer timer_ms;
Timer timer_s; 
Timer timer_pid;
 
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;
int on_line = 0;
static int pos = 0;
unsigned int last_proportional = 0;
long integral = 0;
 
volatile int flag = 0;
volatile int start = 0;
volatile int timer_flag = 0;

int ch;
 
int min_t=0;
int sec_t=0;
int msec_t=0;  

int currTime = 0;
int prevTime = 0;
int dt ;

char data_write[1] ;
char data_read[1] ;
int status;

int count_stop;
int err;
int pid;
 
int readLine(unsigned int *sensor_values);
void tr_ready(void);
void calibration(void);
void init(void);
void i2c_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);
 
void bottom_led(void);
void control(void);
 
int main()
{
    
    pc.printf("start\r\n");
    spi.format(16, 0);
    spi.frequency(2000000);
    
    i2c_init();
    init();
 
    
    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();
    timer_ms.start();
    timer_s.start(); 

    while(!timer_flag);
    oled_thread.start(&oled_disp);
    msec_t = timer_ms.read_ms();
    msec_t %= 1000;
    sec_t = timer_s.read();
    
    if(sec_t > 60){ 
        min_t = 1;
        sec_t %= 60;
    }
    
    while (1);
}


void bottom_led(void){
    
    int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};

    for (int i = 0; i < WS2812_BUF; i++) {
        px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
    }
        
    for (int j=0; j<WS2812_BUF; j++) {
        px.SetI(j%WS2812_BUF, 0x4B);
    }
    
    for (int z=WS2812_BUF; z >= 0 ; z--) {
        ws.write_offsets(px.getBuf(),z,z,z);
        wait(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 = 12.9;
    motor.kd = 2.0;
    motor.ki = 6000;
    motor.max = 180;
 
    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 i2c_init(void){
    
    i2c.frequency(100000);
    data_write[0] = 0xE0; //1110_0000
    
    status = i2c.write(PCF8574_ADDR << 1, data_write, 1, 0);
   
    if(status != 0){
        pc.printf("configuration err\r\n");
        while(1){}    
    }

}
 
void oled_disp(void){
    myOled.begin();
    while(1){
        myOled.printf("timer: %d:%d:%d sec\r",min_t, sec_t, msec_t);
//        myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr); 
        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){
    unsigned int right_max = 0.8;
    unsigned int left_max = 0.805;
//    unsigned int sub = 0.5;
    while(1){
        // read current IR 1 ~ IR 5
        read_ir();
        
        // set range under 1000
        set_ir_val();
    
        // get current position
        get_pos();
        if(pos<1800){ //turn r
            motor.speed_r(right_max);
            motor.speed_l(left_max + 0.13);
        }
        else if(pos>2250){ //turn l
            motor.speed_r(right_max + 0.18);
            motor.speed_l(left_max - 0.1);
        }
        else{
            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;
             
   }
   
   for(int i = 0; i < NUM_SENSORS; i++){
        pc.printf("sensor_values[%d]: %d\r\n",i, sensor_values[i]);   
    }
    pc.printf("\r\n");

   // finish line
   if(sensor_values[0] < 300 && sensor_values[1] < 300 && sensor_values[2] < 300 && sensor_values[3] < 300 && sensor_values[4] < 300){
//        count_stop ++;
//        if(count_stop > 7){
            timer_flag = 1;
            motor.stop();
        
            bottom_led();     
//        } 
    }
//    else{
//        count_stop = 0;
//    }

}
 
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 < 450){
            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
//            pos = 200;
        }
        else{                                    // right -> out-line (over 2000)
             pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
//            pos = 3800;
        }
    }
    // 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;

    const float std = 500.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 > 10){ //0.1 + p_d/std
        motor.speed_r((motor.max + power_difference)/std);
        motor.speed_l((motor.max - power_difference)/std + 0.005);
    }
    // bot position: left
    else if(power_difference < -10){
        motor.speed_r((motor.max + power_difference)/std);
        motor.speed_l((motor.max - power_difference)/std + 0.005);
    }
    else{ //online
        motor.speed_l(motor.init_sp_l);
        motor.speed_r(motor.init_sp_r);
    }    

}