File content as of revision 97:8f3abd5cf5ce:

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

#define button_SENSORS 5
#define ADT7420_TEMP_REG (0x00)
#define ADT7420_CONF_REG (0x03)
#define EVAL_ADT7420_ADDR (0x48)
#define PCF8574_ADDR (0x20)

// 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;

// 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;
static float ival = 0;
static float dval = 2.0;
static float vel = 150.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 main()
{   
    
 //   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);
                wait(0.1);
                button = 0x1C;
                break;
            
            case 0x52:
            // 8 button (move backward)
                motorDriver.user_backward(0.3,0.3);
                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();
                
                while(1)
                {  
                    t=timer.read();
                    update_display();
                    position=trs.readLine(sensorValues,0);
                    
                    /*if(dist < 10)
                    {
                        int count=0;
                        while(count<150){
                            if(dist>10) break;
                            count++;
                        }
                        if(count<150) continue;
                        while(1)
                        {
                            
                            motorDriver.user_left(0.2,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+15)/255,(maximum+power_difference)/255);               
                    else 
                        motorDriver.user_forward((maximum-power_difference)/255,(maximum-15)/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();
                        button = 0x1C;
                        break;
                    }
                }
                break;
                
            case 0x42:
            // 7 button (read sensor values)
                while(1)
                {
                    position=trs.readLine(sensorValues,0);
                    for(int i=0;i<5;i++){
                        pc.printf("%d\r\n",sensorValues[i]);
                    }
                    pc.printf("done!\r\n");
                    wait(1);
                }
                
            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;                
            dafault:
                // wrong button
                pc.printf("wrong button!\r\n");
                break;
        }
    }
}