File content as of revision 96:7465ab270e7a:

#include "TRSensors.h"
#include "ReceiverIR.h"
#include "Drive.h"
#include "Adafruit_SSD1306.h"
#include "WS2812.h"
#include "PixelArray.h"
#include "ultrasonic.h"

#define NUM_SENSOR 5
#define OLED_RESET D9
#define WS2812_BUF 77   //number of LEDs in the array
#define NUM_COLORS 6    //number of colors to store in the array
#define NUM_STEPS 8    //number of steps between colors

#define KP 0.45
#define KI 0.0001
#define KD 0.1

#define PCF8574_ADDR 0x20
#define beep_on  PCF8574Write(0xDF & PCF8574Read())
#define beep_off PCF8574Write(0x20 | PCF8574Read())

// IR REMOTE
ReceiverIR ir_rx(PB_5);
RemoteIR::Format format;
uint8_t buf[8];
int bitlength;

// Control motor drive
Drive drive(D6, D5, A1, A0, A2, A3);
int control_speed = 3000;
int calibrate_speed = 6000;
int running_speed = 4000;

//auto
const int maximum = 1000;
float multiples, multiples2;
int cnt = 0;


// TR Sensor
TRSensors trs = TRSensors(D10, D11, D12, D13);
uint16_t sensorValues[NUM_SENSOR];
unsigned int last_proportional;
long integral;
int last_power_difference = 0;

// IR sensor
I2C i2c_for_irsensor(PB_9, PB_8);

// Ultra sonic sensor
Ultrasonic us(D3, D2, 0.1, false, 0.1);
bool isClose = false;

// OLED time print
I2C i2c_for_oled(I2C_SDA,I2C_SCL);
Adafruit_SSD1306_I2c myOled(i2c_for_oled,OLED_RESET,0x78,64,128);
Timer t;

// LED when in goal
PixelArray px(WS2812_BUF);
WS2812 ws(D7, WS2812_BUF, 6,17,9,14);   //nucleo-f411re // See the program page for information on the timing numbers

// Debug
RawSerial pc(PA_2, PA_3, 115200);

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

void calibration(){
    memset(buf, 0x00, sizeof(buf));
    drive.setSpeed(calibrate_speed);
    while(true)
    {
        bitlength = receive(&format, buf, sizeof(buf));
        if (bitlength < 0) {
            continue;
        }
        
        if(buf[2] == 0x18)
            drive.Forward();
                
        else if(buf[2] == 0x52)
            drive.Backward();
            
        else if(buf[2] == 0x8)
            drive.Turn_left();
                
        else if(buf[2] == 0x5A)
            drive.Turn_right();
            
        else if(buf[2] == 0x1C)
            drive.Break();
            
        else if(buf[2] == 0x0C)
        {
            drive.Break();
            break;
        }
        
        wait(0.1);
        drive.Break();
                
        trs.calibrate();
    }
}

void ready_to_drive(){
    drive.setSpeed(control_speed);
    memset(buf, 0x00, sizeof(buf));
    while(true){
        bitlength = receive(&format, buf, sizeof(buf));
        if (bitlength < 0) {
            continue;
        }
        
        if(buf[2] == 0x18)
            drive.Forward();
                
        else if(buf[2] == 0x52)
            drive.Backward();
            
        else if(buf[2] == 0x8)
            drive.Turn_left();
                
        else if(buf[2] == 0x5A)
            drive.Turn_right();
            
        else if(buf[2] == 0x1C)
            drive.Break();
            
        else if(buf[2] == 0x5E)
        {
            drive.Break();
            break;
        }
        
        wait(0.1);
        drive.Break();
    }
}

void PCF8574Write(char data){
    i2c_for_irsensor.write((PCF8574_ADDR<<1), &data, 1);
}

void PCF8574Initialize(){
    PCF8574Write(0x01);
}

char PCF8574Read(){
    char data = 0xFF;
    i2c_for_irsensor.read(((PCF8574_ADDR<<1)|0x01), &data, 1);
    return data;
}

void check_irsensor(void){
    char data;
    
    PCF8574Write(0xC0 | PCF8574Read());
    data = PCF8574Read() | 0x3F;
    if(data == 0x7F){
        pc.printf("left\r\n");
        // avoid_right_obstacle();
    } else if(data == 0xBF){
        pc.printf("right\r\n");
        // avoid_left_obstacle();
    }
}

void check_obstacle_with_ultrasonic_sensor(void){
    while(true){
        PCF8574Write(0xC0 | PCF8574Read());
        int dist = us.getDistance();
        if(dist < 30 && dist != -1){ // Reduce magic number
//            beep_on;
            isClose = true;
        } else {
//            beep_off;
            isClose = false;
        }
//        myOled.clearDisplay();
//        myOled.setTextCursor(0, 0);
//        myOled.printf("dist: %d\r\n", dist);
//        myOled.display();
        wait(0.05);
    }
}

void start_auto_drive(){
    drive.setSpeed(running_speed);
//       drive.Forward();
    Thread thread;
    thread.start(check_obstacle_with_ultrasonic_sensor);
    
    while(true){
        unsigned int position = trs.readLine(sensorValues);
        int proportional = (int)position - 2000;
        int derivative = proportional - last_proportional;
        integral += proportional;
//        last_proportional = proportional;
    
        int power_difference = proportional*KP + integral*KI + derivative*KD;
        
        // It need to have a specific rule
        if( (sensorValues[0] >= 700) &&
            (sensorValues[1] >= 700) && 
            (sensorValues[2] >= 700) &&
            (sensorValues[4] >= 700) )
        {
            drive.Break();
            break;
        }
        
        if(power_difference > maximum)
            power_difference = maximum;
        if(power_difference < -maximum)
            power_difference = -maximum;
            
        int power_difference_n = power_difference * 2.3;
        float left, right;
        
        // error 보정
        if(power_difference_n < 0)
        {
            left = control_speed;
            right = control_speed-power_difference_n;
        }
        else
        {
            left = control_speed+power_difference_n;
            right = control_speed;
        }
        
        if(position < 900) {
            multiples2 = 0.5f;
            left = 0;
            cnt = 0;
        } else if(position < 1300) {
            multiples2 = 0.5f;
            left = 0;
            cnt = 0;
        } else if(position < 1550) {
            multiples2 = 0.5f;
            left = 0;
            cnt = 0;
        } else if(position < 2000) {
            if(cnt < 5) multiples2 = 0.5f;
            else multiples2 = 1.0f;
            cnt++;
        } else if(position < 2350) {
            if(cnt < 5) multiples2 = 0.5f;
            else multiples2 = 1.0f;
            cnt++;
        } else if(position < 2500) {
            multiples2 = 0.5f;
            right = 0;
            cnt = 0;
        } else if(position < 3100) {
            multiples2 = 0.5f;
            right = 0;
            cnt = 0;
        } else {
            multiples2 = 0.5f;
            right = 0;
            cnt = 0;
        }
        
        if(isClose) {
            multiples = 1.2f * multiples2;
            if((sensorValues[0] >= 500) || (sensorValues[1] >= 500)){
                do{
                    drive.setSpeed(0, 5000);
                    drive.Forward();
                    wait(0.01);
                    drive.Break();
                    position = trs.readLine(sensorValues);
                } while(sensorValues[2] >= 700);
                continue;
            }
        } else {
            multiples = 2.4f * multiples2;
        }
        
        last_proportional = proportional;
        
        left *= multiples;
        right *= multiples;
        
        drive.setSpeed((int)left, (int)right);
        drive.Forward(); 

        wait(0.005);
        drive.Break();
        
//        myOled.clearDisplay();
//        myOled.setTextCursor(0, 0);
//        // for oled debug
//        for(int i=0; i<5; i++){
//            myOled.printf("%d\t", sensorValues[i]);
//            myOled.display();
//        }
//        
//        myOled.printf("\r\np:%d\r\npo:%d\r\n", position, power_difference);
//        myOled.display();
//        myOled.printf("\r\nleft:%d\r\nright:%d\r\n", (int)left, (int)right);
//        myOled.display();
    }
}


int color_set(uint8_t red,uint8_t green, uint8_t blue)
{
  return ((red<<16) + (green<<8) + blue);   
}

// 0 <= stepNumber <= lastStepNumber
int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
{
    return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
}


void bottom_led(void){  //led 제어 
    int colorIdx = 0;
    int colorTo = 0;
    int colorFrom = 0;
    
    uint8_t ir = 0;
    uint8_t ig = 0;
    uint8_t ib = 0;
    
       
    ws.useII(WS2812::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};
    
    // Now the buffer is written, write it to the led array.
    while (true) 
    {
        //get starting RGB components for interpolation
        std::size_t c1 = colorbuf[colorFrom];
        std::size_t r1 = (c1 & 0xff0000) >> 16;
        std::size_t g1 = (c1 & 0x00ff00) >> 8;
        std::size_t b1 = (c1 & 0x0000ff);
        
        //get ending RGB components for interpolation
        std::size_t c2 = colorbuf[colorTo];
        std::size_t r2 = (c2 & 0xff0000) >> 16;
        std::size_t g2 = (c2 & 0x00ff00) >> 8;
        std::size_t b2 = (c2 & 0x0000ff);
        
        for (int i = 0; i <= NUM_STEPS; i++)
        {
            ir = interpolate(r1, r2, i, NUM_STEPS);
            ig = interpolate(g1, g2, i, NUM_STEPS);
            ib = interpolate(b1, b2, i, NUM_STEPS);
            
            //write the color value for each pixel
            px.SetAll(color_set(ir,ig,ib));
            
            //write the II value for each pixel
            px.SetAllI(32);
            
            for (int i = WS2812_BUF; i >= 0; i--) 
            {
                ws.write(px.getBuf());
            }
        }
        
        colorFrom = colorIdx;
        colorIdx++;
        
        if (colorIdx >= NUM_COLORS)
        {
            colorIdx = 0;
        }
        
        colorTo = colorIdx;
        
    }
}

int main(){
    //program setupa
    myOled.begin();
    i2c_for_irsensor.frequency(100000);    // main에 써야함  
    PCF8574Initialize();
    beep_off;
    PCF8574Write(0xC0 | PCF8574Read());
    
    // Test OLED
    myOled.printf("oled size: %ux%u \r\nStart Alphabot2\r\n", myOled.width(), myOled.height());
    myOled.display();
    
    //program start
    memset(buf, 0x00, sizeof(buf));
    
    while(true){
        bitlength = receive(&format, buf, sizeof(buf));
        if (bitlength < 0) {
            continue;
        }
        
        if(buf[2] == 0x42)
            break;
    }
    
    //calibration
//    pc.printf("calibration!\r\n");
    myOled.printf("calibration!\r");
    myOled.display();
    calibration();

    //ready autodriving
//    pc.printf("ready_to_drive!\r\n");
    myOled.printf("ready_to_drive!\r");
    myOled.display();
    ready_to_drive();

    //go autodrive
//    pc.printf("start_auto_drive!\r\n");
    myOled.printf("start_auto_drive!\r\n");
    myOled.display();
    t.start();
    start_auto_drive();
    t.stop();
    beep_off;
    pc.printf("The time taken was %f seconds\r\n", t.read());
    
    /* OLED time print */
    myOled.printf("\nGoal! \r\nThe time taken was\r\n%f seconds\r\n", t.read());
    myOled.display();
    /* LED run*/ 
    Thread thread;
    thread.start(bottom_led);
//    bottom_led();
    
    while(true){
        beep_off;
        myOled.display();    
    }
}