File content as of revision 99:08d58c8231ca:

#include "mbed.h"
#define button2 18
#include "TB6612FNG.h"
#include "HCSR04.h"
#include "ReceiverIR.h"
#include "TransmitterIR.h"
#include "Adafruit_SSD1306.h"   //OLED
#define I2C_ADDR (0x20)

#include "WS28121.h"
#include "PixelArray.h"
#define NUM_LEDS_PER_COLOR 4
#define NUM_COLORS 6    //number of colors to store in the array
#define WS2812_BUF 77   //number of LEDs in the array
uint32_t colors[4]={0xffffff,0xffffff,0xffffff, 0xffffff}; // led1, led2, led3, led4 --> 각 LED color hex 값

PixelArray px(WS2812_BUF);

// See the program page for information on the timing numbers
WS28121 ws(D7, WS2812_BUF, 6,17,9,14);   //nucleo-f411re

ReceiverIR ir_rx(D4);
Thread sock_thread;
Thread cali_thread;
I2C i2c(I2C_SDA, I2C_SCL);
SPI spi(D11,D12,D13);
DigitalOut spi_cs(D10,1);
RawSerial pc(SERIAL_TX, SERIAL_RX,115200);
TB6612FNG motorDriver(D6, A0, A1, D5, A3, A2, A5);
Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); 
Thread LED_thread;


void calibrate();
char rx_buffer[10];
int index = 0;
volatile int flag = 0;
Timer t;
int result[6];
bool istart = false;
int calibratedMax[5] = {0,};
int calibratedMin[5] = {1023,};
int sensor_values[5]= {0,0,0,0,0};
int hcsr04();
int start, end;

HCSR04 sensor(D3, D2, pc, 0.5);

int hcsr04(){
    int distance;
    sensor.setMode(false);
    distance = sensor.distance();
    sensor.clearStatus();
            
    return distance;
}

void moveStop(void)
{
    motorDriver.motorB_stop();
    motorDriver.motorA_stop();

}

void moveBackward(float t)
{
    motorDriver.motorA_ccw();
    motorDriver.motorB_ccw();
    //wait(t);    
}


void moveForward(float t)
{
    motorDriver.motorA_cw();
    motorDriver.motorB_cw();
    //wait(t); ////
    //moveStop();////
}

void moveLeft(float t)
{
    motorDriver.motorA_ccw();
    motorDriver.motorB_cw();
    wait(t);
    moveStop();////
}


void moveRight(float t)
{
    motorDriver.motorA_cw();
    motorDriver.motorB_ccw();
    wait(t);
    moveStop();////
}


int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) {
    int cnt = 0;
    while (ir_rx.getState() != ReceiverIR::Received) {
        cnt++;
        if (timeout < cnt) {
            return -1;
        }
    }
    return ir_rx.getData(format, buf, bufsiz * 8);
}



/**
 * Display a data.
 *
 * @param buf Pointer to a buffer.
 * @param bitlength Bit length of a data.
 */



void set_TLC1543(void)
{
    int i;
    for(i = 0; i < 6; i++){
        int value = 0;    
        spi_cs = 0;
        wait_us(2);
        value = spi.write(i<<12);
        spi_cs = 1;
        wait_us(18);  
        value = value >> 6;
        result[i] = value;
    }
    
    for(int i=0; i<5; i++){ 
        sensor_values[i] = result[i+1];
       // pc.printf("sensor_values[i] = %d\r\n", i, sensor_values[i]);
    }
    
}

void calibrate()
{
    int x;
    int max_sensor_values[5] = {0,};
    int min_sensor_values[5] = {1024,};
    
    int q;
    for(q=0;q<30;q++){
        wait(0.5);
        printf("count cal : %d\r\n",q);
        set_TLC1543();
        for(x=0;x<5;x++){
            if(q==0 || max_sensor_values[x] < sensor_values[x])
                max_sensor_values[x] = sensor_values[x];
                
            if(q==0 || min_sensor_values[x] > sensor_values[x])
                min_sensor_values[x] = sensor_values[x];
        }

    }

    for(int i=0; i<5; i++)
    {
        if(min_sensor_values[i] > calibratedMax[i])
            calibratedMax[i] = min_sensor_values[i];
        if(max_sensor_values[i] < calibratedMin[i])
            calibratedMin[i] = max_sensor_values[i];
    }
    
      for(int i = 0 ; i < 5; i++){
        pc.printf("cali-Max[%d] : %d\r\n",i,calibratedMax[i]);
        pc.printf("cali-Min[%d] : %d\r\n",i,calibratedMin[i]);
    }
    
    myOled.printf("End Calibrate\r\n",end-start);
    myOled.display(); 
}

void readCalibrated(int *sensor_values)
{
    int i;
    
    set_TLC1543();
    
    for(i=0; i<5; i++)
    {
        int denominator;
        
        denominator = calibratedMax[i] - calibratedMin[i];
        
        int x=0;
        if(denominator != 0)
            x = (sensor_values[i] - calibratedMin[i])*1000/denominator;
            
        if(x<0)
            x=0;
        else if(x>1000)
            x=1000;
        sensor_values[i] = x;
    }
}

int readLine(int *sensor_values)
{
    char i, on_line = 0;
    long avg; // this is for the weighted total, which is long
                       // before division
    int sum; // this is for the denominator which is <= 64000
    static int last_value=0; // assume initially that the line is left.

    readCalibrated(sensor_values);

    avg = 0;
    sum = 0;
   int _numSensors = 5;
  
    for(i=0;i<_numSensors;i++) {
        int value = sensor_values[i];
          pc.printf("IR%d : %d\r\n",i,value);
    
    
        sensor_values[i] = value;
        // keep track of whether we see the line at all
        if(value > 300) {
            on_line = 1;
        }
        
        // only average in values that are above a noise threshold
        if(value > 50) {
            avg += (long)(value) * (i * 1000);
            sum += value;
        }
    }

    if(!on_line)
    {
        // If it last read to the left of center, return 0.
         if(last_value < (_numSensors-1)*1000/2)
             return 0;
        
        // If it last read to the right of center, return the max.
         else
             return (_numSensors-1)*1000;
    }

    last_value = avg/sum;
    if(sum > 4500){
        last_value = -1;    
    }

    return last_value;
}

void rx_cb(void)
{
    char ch;
    ch = pc.getc();
    pc.putc(ch);
    rx_buffer[index++] = ch;
    if (ch == 0x0D) { //CR
        pc.putc(0x0A); //LF
        rx_buffer[--index] = '\0'; //change CR to 0
        index = 0;
        flag = 1;
    }
}

void turnonLED(){
     ws.useII(WS28121::PER_PIXEL); // use per-pixel intensity scaling
    
    // set up the colours we want to draw with
    int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};

    // for each of the colours (j) write out 10 of them
    // the pixels are written at the colour*10, plus the colour position
    // all modulus 60 so it wraps around
    for (int i = 0; i < WS2812_BUF; i++) {
        px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
    }

    // now all the colours are computed, add a fade effect using intensity scaling
    // compute and write the II value for each pixel
    for (int j=0; j<WS2812_BUF; j++) {
        // px.SetI(pixel position, II value)
        px.SetI(j%WS2812_BUF, 0xf+(0xf*(j%NUM_LEDS_PER_COLOR)));
    }


    // Now the buffer is written, rotate it
    // by writing it out with an increasing offset
    while (1) {
        for (int z=WS2812_BUF; z >= 0 ; z--) {
            ws.write_offsets(px.getBuf(),z,z,z);
            wait(0.075);
        }
    }
}

void display_data(uint8_t *buf, int bitlength) {

    const int n = bitlength / 8 + (((bitlength % 8) != 0) ? 1 : 0);

    switch(buf[3]){
        case 0xF3:  // 1, forward
            moveForward(0.2);
            wait(0.2);
            moveStop();
            break;
        case 0xE7:  //2, stop
            moveStop();
            break;
        case 0xA1: // 3, backward
            moveBackward(0.2);
            wait(0.2);
            moveStop();
            break;
        case 0xF7:  // 4, right
            moveRight(0.1);
            break;
        case 0xE3:  // 5, left
            moveLeft(0.1);
            break;
        case 0xBD:  //  7
            moveForward(0.05);
            wait(0.05);
            moveStop();
            break;
        case 0xAD:  // 8
            moveBackward(0.05);
            wait(0.05);
            moveStop();
            break;
        case 0xB5:  // 9
            motorDriver.motorA_ccw();
            motorDriver.motorB_cw();
            break;
        
        case 0xBA: // ch-
            moveForward(0.1);
            wait(0.1);
            moveStop();
            cali_thread.start(&calibrate);

            break;
        
        case 0xB9: // ch
            pc.printf("start!\r\n");
            char data_write[2];
            char data_read[2];
            
            data_write[0] = 0xE0; 
            i2c.write((I2C_ADDR << 1), data_write, 1);
            i2c.frequency(100000); // 100 k
            
            t.start();
            start = t.read_ms();
            while(1) {
                    pc.printf("--------------------\r\n");
                    set_TLC1543();
                    int position = readLine(sensor_values);
                    pc.printf("position : %d\r\n", position);
                    int distance = hcsr04();
                    pc.printf("distance:     %d\r\n",distance);
                    i2c.read(((I2C_ADDR << 1) | 0x01), data_read, 1, 0);
                    int tempval = data_read[0];
                    /*
                        if(tempval == 0x60){
                        pc.printf("+++++++++left++++++++++\r\n"); //left
                        moveRight(0.06);
                        wait(1);
                        moveForward(0.21 );
                        wait(1);
                        moveLeft(0.06);
                        do{
                            moveForward(0.06);
                            wait(0.2);
                            readCalibrated(sensor_values);
                            wait(0.3);   
                        }while(sensor_values[3] <700);
                        
                        continue;
                    }
                    else if(tempval == 0xA0){
                        pc.printf("===========right========\r\n"); //right
                        moveLeft(0.06);
                        wait(1);
                        moveForward(0.22 );
                        wait(1);
                        moveRight(0.09);
                        do{
                            moveForward(0.06);
                            wait(0.2);
                            readCalibrated(sensor_values);
                            wait(0.3);   
                        }while(sensor_values[3] <700);
                        
                        continue;
                    }
                    */
                    /*
                    if(tempval == 0x60){
                        pc.printf("+++++++++left++++++++++\r\n"); //left
                        moveRight(0.06);
                        wait(1);
                        moveForward(0.24 );
                        wait(1);
                        moveLeft(0.06);
                        do{
                            moveForward(0.06);
                            wait(0.2);
                            readCalibrated(sensor_values);
                            wait(0.3);   
                        }while(sensor_values[3] <700);
                        
                        continue;
                    }
                    else if(tempval == 0xA0){
                        pc.printf("===========right========\r\n"); //right
                        moveLeft(0.06);
                        wait(1);
                        moveForward(0.24 );
                        
                        wait(1);
                        moveRight(0.09);
                        do{
                            moveForward(0.06);
                            wait(0.2);
                            readCalibrated(sensor_values);
                            wait(0.3);   
                        }while(sensor_values[3] <700);
                        
                        continue;
                    }
                    
                    */
                    
                    if(distance <=21){
                        pc.printf("stop!!!!please!!\r\n");
                        moveStop(); ////
                        moveLeft(0.09);
                    }
                    
                    if(position >= 1750 && position < 2600){
                     moveForward(0.08);
                    }
                    else if(position >= 2600 && position < 3000){
                     moveRight(0.04);   
                     moveStop();////
                    }
                    else if(position <= 3500 && position >= 3000){
                     moveRight(0.03);
                    }
                    else if(position <= 4000 && position > 3500){
                     moveRight(0.02);
                    }
                    else if(position < 1750 && position > 1000){
                     moveLeft(0.04);  
                    }
                    else if(position <= 1000 && position > 0){
                     moveLeft(0.03);    
                    }
                    else if(position == 0){
                     moveLeft(0.02);
                    }
                    else if(position == -1){
                     moveStop();
                     end = t.read_ms();
                     pc.printf("stop!!!!\r\n"); 
                     pc.printf("Total time is %d ms.\r\n", end-start);
                     
                     myOled.printf("Total time is %d ms.\r\n",end-start);
                     myOled.display();    
                     LED_thread.start(&turnonLED);
                     
                     wait(5);                  
                    }
                    wait(0.1);
              }
            break;
        }

}
void rx_thread() {
      while (1) {
        wait(0.2);
        
        uint8_t buf1[32];
        int bitlength1;
        RemoteIR::Format format;

        memset(buf1, 0x00, sizeof(buf1));
         
        bitlength1 = receive(&format, buf1, sizeof(buf1));
        if (bitlength1 < 0) {
            continue;
        }      
        display_data(buf1, bitlength1);
    }
}

int main()
{
  float fPwmPeriod = 0.00002f;//50KHz
  float fPwmPulsewidth = 0.250;//duty cycle 0.0~1.0 ->  0%~100%
    ///
  motorDriver.setPwmAperiod(fPwmPeriod);//set PWM Period
  motorDriver.setPwmBperiod(fPwmPeriod);
    
  motorDriver.setPwmApulsewidth(fPwmPulsewidth);//set PWM Puls Width
  motorDriver.setPwmBpulsewidth(fPwmPulsewidth);

  sock_thread.start(&rx_thread);
    
  spi.format(16, 0);
  spi.frequency(2000000);
  
  pc.printf("Hello!\r\n");
  pc.attach(callback(rx_cb));

  //OLED
  myOled.begin();
  myOled.printf("\nHello World\r\n", myOled.height());
  myOled.display();

    for(int i = 0; i < 5; i++)
        calibratedMin[i] = 1023;
  //fin thread
  sock_thread.join();
  cali_thread.join();
  LED_thread.join();
}