File content as of revision 98:d8ead5e04047:

#include "mbed.h"
#include "RemoteIR.h"
#include "ReceiverIR.h"
#include "TB6612FNG.h"
#include "TRSensors.h"
#include "ultrasonic.h"
#include "Adafruit_SSD1306.h"
#include "WS2812.h"
#include "PixelArray.h"

#define button_SENSORS 5
#define ADT7420_TEMP_REG (0x00)
#define ADT7420_CONF_REG (0x03)
#define EVAL_ADT7420_ADDR (0x48)
#define PCF8574_ADDR (0x20)
#define WS2812_BUF 100
#define WS2812_BUF2 4
#define NUM_COLORS 3
#define NUM_LEDS_PER_COLOR 4

// create object
DigitalOut dc(D8,1);
DigitalOut rst(D9,1);
I2C i2c(I2C_SDA, I2C_SCL);
TB6612FNG motorDriver(D6, A1, A0, D5, A2, A3);
Ultrasonic ultra(D3, D2, .1, false);
ReceiverIR IR(D4);
TRSensors trs;
RawSerial pc(USBTX, USBRX, 115200);
Adafruit_SSD1306_I2c gOled2(i2c,D9,0x7A,64,128);
Ticker IRticker;
Thread thread1;
Thread thread2;
Timer timer;
PixelArray px(WS2812_BUF);

// See the program page for information on the timing numbers
// The given numbers are for the K64F
WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);

// variables
unsigned int sensorValues[button_SENSORS]; 
unsigned int last_proportional = 0;
long integral = 0;
static int prev_err = 0;
static int int_err = 0;
static float pval = 0.15;
static float ival = 0.00003;
static float dval = 2.3;
static float vel = 120.0;
uint8_t IR_buf[64];
int length = 32;
volatile int button = 0;
int position = 0;
int dist = 0;
int cnt = 0;
int err = 0;
float t=0;

// Reflective sensor
bool rightObs= false;
bool leftObs = false;

// motor Driver
float fPwmAPeriod;
float fPwmAPulsewidth;
float fPwmBPeriod;
float fPwmBPulsewidth;

// opearte ultrasonic
//void ultrasonic_thread()
//{
//    while (1)
//    {
//        ultra.setMode(false);           // have updates every .1 seconds and try only once.
//        ultra.trig();
//        dist = ultra.getDistance();
//        pc.printf("Detected value: %d\r\n", dist);
//    }
//}


// opearte reflective sensors
/*
void reflective_thread()
{
    while(1){
        char data_write[2];
        char data_read[2];
        i2c.frequency(100000);
        i2c.read((PCF8574_ADDR<<1|0x01), data_read, 2, 0);
        pc.printf("hexa: %x, %x\r\n", data_read[0], data_read[1]);
        if(data_read[0]>>6 == 0b10)
        {
            pc.printf("Obstacle on left!\r\n");
            data_write[0] = 0xDF;
            leftObs = true;
        }
        else if(data_read[0]>>6 == 0b01)
        {
            pc.printf("Obstacle on right!\r\n");
            data_write[0] = 0xDF;
            rightObs = true;
        }
        else if(data_read[0]>>6 == 0b00)
        {
            pc.printf("Obstacle on front!\r\n");
            data_write[0] = 0xDF;
            leftObs = true;
            rightObs = true;
            wait(3);
        }
        
        else
        {
            data_write[0]=0xFF;
        }
        i2c.write((PCF8574_ADDR<<1),data_write,1,0);      
    }
}*/

inline void update_display(){
                gOled2.clearDisplay();
                gOled2.setTextCursor(0,0);
                gOled2.printf("E-RON alphabot\r\n");
                gOled2.printf("Timer: %.2f\r\n",t);
                gOled2.printf("V: %.0f\r\n",vel);
                gOled2.printf("P: %.2f\r\n",pval);
                gOled2.printf("I: %.5f\r\n",ival);
                gOled2.printf("D: %.2f\r\n",dval);
                gOled2.display();
}
    int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
    int colorbuf3 =0x000000;
int main()
{      
    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(pixel position, II value)
        px.SetI(j%WS2812_BUF, 0xf+(0xf*4));
    }
    
 //   thread1.start(ultrasonic_thread);
 //   thread2.start(reflective_thread);
    RemoteIR::Format format;
    uint8_t buf[32];
    int bitcount;
    update_display();
    while(1)
    { // read the value of the code
        if (IR.getState() == ReceiverIR::Received)
        {
            bitcount = IR.getData(&format, buf, sizeof(buf) * 8);
            pc.printf("%d\r\n", bitcount);
            pc.printf("buf[2]: %d\r\n", buf[2]);
            button = buf[2];
        }
        
        switch(button)
        {
            case 0x45:
            // ch- button (P value up)
                button = 0x1C;
                pval += 0.01;
                pval = fabs(pval);
                update_display();
//                pc.printf("pval:%f\r\n", pval);
                wait(0.3);
                break;
            
            case 0x46:
            // ch common button (I value up)
                button = 0x1C;
                ival += 0.00001;
                ival = fabs(ival);
                update_display() ;        
//                pc.printf("ival:%f\r\n", ival);
                wait(0.3);
                break;
            
            case 0x47: 
            // ch+ button (D value up)
                button = 0x1C;
                dval += 0.05;
                dval = fabs(dval);
                update_display();
                wait(0.3);
                break;
                
            case 0x44:
            // prev button (P value down)
                button = 0x1C;
                pval -= 0.01;
                pval = fabs(pval);
                update_display();
                wait(0.3);
                break;
            
            case 0x40:
            // next button (I value down)
                button = 0x1C;
                ival -= 0.00001;
                ival = fabs(ival);
                update_display();
                wait(0.3);
                button = 0x1C;
                break;
                
            case 0x43:
            // play/pause button (D value down)
                button = 0x1C;
                dval -= 0.05;
                dval = fabs(dval);
                update_display();
                wait(0.3);
                button = 0x1C;
                break;

            case 0x07:
            // vol- button (velocity down)
                button = 0x1C;
                vel -= 5;
                update_display();
                wait(0.3);
                button = 0x1C;
                break;
                
            case 0x15:
            // vol+ button (velocity up)
                button = 0x1C;
                vel += 5;
                update_display();
                wait(0.3);
                button = 0x1C;
                break;
                
            case 0x16:
            // 0 button (Reset the error variables)
                int_err = 0;
                err = 0;
                prev_err = 0;
                wait(1);
                button = 0x1C;
                break;
            
            case 0x19:
            // 100+ button (Calibrate)
                pc.printf("calibrate!\r\n");
                trs.calibrate();
                pc.printf("calibrate done\r\n");
                wait(0.2);
                button = 0x1C;
                break;
                
            case 0x18:
            // 2 button (move forward)
                motorDriver.user_forward(0.3,0.3);
                trs.calibrate();
                wait(0.1);
                button = 0x1C;
                break;
            
            case 0x52:
            // 8 button (move backward)
                motorDriver.user_backward(0.3,0.3);
                trs.calibrate();
                wait(0.1);
                button = 0x1C;
                break;
            
            case 0x5A:
            // 6 button (clockwise turn)
                motorDriver.user_right(0.3,0.1);
                wait(0.1);
                button = 0x1C;
                break;
            
            case 0x08:
            // 4 button (counter clockwise turn)
                motorDriver.user_left(0.1,0.3);
                wait(0.1);
                button = 0x1C;
                break;
            
            case 0x1C:
            // 5 button (motor stop)
                motorDriver.stop();
                break;
            
            case 0x0D:
            // 200+ button (line tracer)
                t=0;
                timer.reset();
                timer.start();
                ultra.setMode(false);           // have updates every .1 seconds and try only once.

                while(1)
                {  
                    t=timer.read();
                    position=trs.readLine(sensorValues,0);
                    ultra.trig();
                    dist = ultra.getDistance();
                    pc.printf("dist:%d\r\n", dist);
                    if(dist<=21){
                          motorDriver.user_left(0.2,0.2);
                          wait(0.1);
                          while(1){
                           
                           // pc.printf("distance: %d\r\n", dist);
                           // pc.printf("position: %d\r\n", position);
                            position=trs.readLine(sensorValues,0);
                            if(position > 2000)
                            {
                                int_err = 0;
                                err = 0;
                                prev_err = 0;
                                break;
                                
                                
                            }
                        }
                        
                    }
                    //if(dist < 25)
//                    {
//                        int count=0;
//                        while(count<100){
//                            if(dist>10) break;
//                            count++;
//                        }
//                        if(count<100) continue;
//                        while(1)
//                        {
//                            
//                            motorDriver.user_forward(0,0.2);
//                            
//                            //pc.printf("distance: %d\r\n", dist);
//                            pc.printf("position: %d\r\n", position);
//                            position=trs.readLine(sensorValues,0);
//                            if(position > 2000)
//                            {
//                                int_err = 0;
//                                err = 0;
//                                prev_err = 0;
//                                break;
//                            }
//                        }
//                    }       
                    
                    
                    err=(int)position-2000; // error>0 --> right, error<0 --> left
                    int_err+=err;

                    const float kp = pval;
                    const float kd = dval;
                    const float ki = ival;
            
                    float power_difference = kp*err + kd*(err-prev_err) + ki*int_err; //+derivative; //error를 누적하는 것
                    prev_err = err;
            
                    const float maximum=vel;
                    if(power_difference > maximum)
                        power_difference = maximum;
                    if(power_difference < -maximum)
                        power_difference = -maximum;      
                        
                    if(power_difference<0)
                        motorDriver.user_forward((maximum-10)/255,(maximum+power_difference)/255);               
                    else 
                        motorDriver.user_forward((maximum-power_difference)/255,(maximum+10)/255); 
                    
                    pc.printf("position value: %d\r\n", position);
                    
                    //pc.printf("cnt: %d\r\n", cnt);
                    for(int i=0;i<5;i++){
                        pc.printf("%d\r\n",sensorValues[i]);
                    }
                    if((sensorValues[0] > 650) && (sensorValues[1]>650) && (sensorValues[2]>650) && (sensorValues[3]>650) && (sensorValues[4]>650))
                    {   // 5 IR sensor are on black
                        timer.stop();
                        t=timer.read();
                        update_display();
                        motorDriver.stop();
                        for(int z=48;z>=0;z=z-4){
                        ws.write_offsets(px.getBuf(),z,z,z);
                        wait(0.1);
                        }
                        
                        button = 0x1C;
                        break;
                    }
                }
                break;
                
            case 0x42:
            // 7 button (read sensor values)
                position=trs.readLine(sensorValues,0);
                for(int i=0; i<5; i++) {
                    pc.printf("%d\r\n",sensorValues[i]);
                }
                pc.printf("done!\r\n");
                button = 0x1C;
                break;

                
            case 0x4A:
            // 9 button (read position)
                int j = 0;
                while(j!=100)
                {
                    j++;
                    position=trs.readLine(sensorValues,0);
                    pc.printf("%d\r\n",position);
                } 
                button = 0x1C;
                break;                
            default:
                // wrong button
                pc.printf("wrong button!\r\n");
                break;
        }
    }
}