File content as of revision 97:59d348745d96:

#include "mbed.h"
#include <iostream>
#include "HCSR04.h"
#include <list>
#include <string.h>
#include <IRremote.h>
#include "TB6612FNG.h"
//#define PCF8574_ADDR     (0x40) 
#define LEFT 127
#define RIGHT 191
#define NUMSENSORS 5
#define MAX 1023
#define MIN 0
#define TRU 1
#define FALS 0
#define MaxSpeed 255// max speed of the robot
#define BaseSpeed 155 
#define speedt 180

#include "SSD1306-Library.h"
#include "FreeSans9pt7b.h"
#include <time.h>
#include <math.h>

#define KEY2 0xFF18E7                 //Key:2 
#define KEY8 0xFF52AD                 //Key:8 
#define KEY4 0xFF08F7                 //Key:4 
#define KEY6 0xFF5AA5                 //Key:6 
#define KEY1 0xFF0CF3                 //Key:1 
#define KEY3 0xFF5EA1                 //Key:3 
#define KEY5 0xFF1CE3                 //Key:5


//I2C i2c(I2C_SDA, I2C_SCL);  //D15,D14
SPI spi(D11, D12, D13);
DigitalOut cs(D10,1);
Serial pc(PA_2, PA_3, 115200); //D1 D0
IRrecv irrecv(D4);


int mode=1;
int index=0;
int rx_buffer[10];
char arr[16];
volatile int flag = 0;
float interval=0.1; //100ms
decode_results result;



/////////sensors////////////

int temp=0;
int calibratedMin[NUMSENSORS];
int calibratedMax[NUMSENSORS];
unsigned int sensor_values[NUMSENSORS];
int in_line[NUMSENSORS];
int ultra_flag=0;
float position;

/////////sensors////////////

/////////Ultrasonic////////////
Ultrasonic ultra(D3, D2);
int ultra_result=10;
/////////Ultrasonic////////////


//////////motor////////////
PwmOut      motor_pwmA(PB_10); //D6
PwmOut      motor_pwmb(PB_4);  // D5
AnalogIn rightMotor2(A0);
AnalogIn rightMotor1(A1);
AnalogIn leftMotor2(A3);
AnalogIn leftMotor1(A2);

TB6612FNG leftmotor(PB_10, A1, A0);
TB6612FNG rightmotor(PB_4, A2, A3);
float last_proportional;
float integral;
float kp = 0.5;
float kd = 0.1;
int lastError;
//////////motor////////////

void IRForward(void)
{
    leftmotor.setSpeed(0.1);
    rightmotor.setSpeed(0.1);

}

void ultraLeft(){
    leftmotor.setSpeed(-0.13);
    rightmotor.setSpeed(0.15);
}

void left(void)
{
    leftmotor.setSpeed(0.03);
    rightmotor.setSpeed(0.15);
}


void right(void)
{
    leftmotor.setSpeed(0.15);
    rightmotor.setSpeed(0.03);
}

void stop(void)
{
    leftmotor.setSpeed(0);
    rightmotor.setSpeed(0);
}

void CalibrationMotor(void)
{
    leftmotor.setSpeed(0.15);
    rightmotor.setSpeed(-0.15);
}

void backward(void)
{
   leftmotor.setSpeed(-0.1);
   rightmotor.setSpeed(-0.1);
}


//bool pcf8574_write(uint8_t data){
//    return i2c.write(PCF8574_ADDR, (char*) &data, 1, 0) == 0;
//}
//
//bool pcf8574_read(uint8_t* data){
//    return i2c.read(PCF8574_ADDR, (char*) data, 1, 0) == 0;
//}
//
//int pcf8574_test(uint8_t value){
//    
//    int ret;
//    uint8_t data=0;
//    
//    ret = pcf8574_write(value);
//    if(!ret) return -1;
//    
//    ret = pcf8574_read(&data);
//    if(!ret) return -2;
//    
//    return data;
//}
//
//void detected(void)
//{
//    pcf8574_test(0xff);
//    while(pcf8574_test(0xff)<255) pcf8574_test(0x00);
//}

int state=0; //0:forward / 1:left / 2:right
int cnt=0;
 int period_us;
    int beat_ms;

void TRSensors(void) {
    
    for(int i=0; i< NUMSENSORS ; i++) {
            calibratedMin[i]= MAX;
            calibratedMax[i] = MIN;
    }
        
}

void set_TLC1543 (void) {
   
   int value;
   for(int i=0; i<6 ; i++){
    cs=0;
    wait_us(2);
    value=spi.write(i<<12);
    cs=1;
    wait_us(21);
    cs=0;
    wait_us(2);
    value=spi.write(i<<12);
    //pc.printf("%d th : 0x%X\r\n", i, value);
    cs=1;
    wait_us(21);
    sensor_values[i]=value;
    }   
}

void calibrate(void){
    int i;

    unsigned int max_sensor_values[NUMSENSORS];
    unsigned int min_sensor_values[NUMSENSORS];   
    
    int j;
    for(j=0; j <10 ; j++){
         set_TLC1543();
         for(i=0; i<NUMSENSORS; i++){
         if(j==0 || max_sensor_values[i] < sensor_values[i]) max_sensor_values[i] = sensor_values[i];
         if(j==0 || min_sensor_values[i] > sensor_values[i]) min_sensor_values[i] = sensor_values[i];
         }
    }
    
    for(i=0; i<NUMSENSORS; 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];
    }

}

void readCalibrated(){
    int i;
    set_TLC1543();
    for(i=0; i<NUMSENSORS ; i++){
        //unsigned int calmin, calmax;
        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;
        else sensor_values[i] = x;
    }    
}

int readLine() {
    unsigned char i, on_line =0;
    unsigned long avg;
    unsigned int sum;
    static int last_value=0;
    
    readCalibrated();
    avg =0; sum =0;
    for(i=0; i< NUMSENSORS; i++){
        int value = sensor_values[i];
        value = 1000-value;
        sensor_values[i]=value;
        in_line[i]=FALS;
        if(value > 300) {
            on_line =1;
            in_line[i]=TRU;
        }  
        //if(value > calibratedMin[i]) on_line=1;
        if(value > 50){
        avg += (long)(value)*(i*1000);
        sum += value;    
        }         
    }
    
    if(!on_line){
        if(last_value < (NUMSENSORS-1)*1000/2) return 0;
        else return ( NUMSENSORS-1 ) * 1000;   
    }
   
   last_value = avg/sum;
   
   return last_value;
            
}

/****************************************************************
before running, set up tlc1543, calibrate **********************/
void settlc1543(){
    cs =1;
    spi.format(16,0);
    spi.frequency(2000000);
    pc.printf("test\r\n");
    //pc.attach(callback(&rx_cb));
    
    TRSensors();
    for(int i=0; i<10; i++) {
        calibrate();
        wait(0.4);
    }
//    pc.printf("calibrate done\r\n");
    for(int i=0; i<NUMSENSORS; i++) {
//        pc.printf("Calibration Min: %d Max: %d\r\n", calibratedMin[i], calibratedMax[i] );
    }
}


void rx_cb(void) {
  
    int ch;
    ch = pc.getc();
    rx_buffer[index]=ch;
    index++;
    pc.putc(ch); 
   if(ch==0x0D) {
    pc.putc(0x0A);
    flag=1;
    index=0;
    }

    
}


int main()
{
    clock_t t;
    int f;
    char displaySentence[40] = "Time is ";
    char time[20];
    irrecv.enableIRIn();
//    mu.startUpdates();//start mesuring the distance
    int data;
    
    SSD1306 display = SSD1306();
    
    while(1){
            
            if(irrecv.decode(&result)){
            pc.printf("%X\r\n",result.value);
            irrecv.resume();
            
            if(result.value == KEY1){       // 1
                break;
            }
            if(result.value == KEY2){       // 2
               IRForward();
            }            
            if(result.value == KEY3){       // 3
                CalibrationMotor();
                settlc1543();
                
            }
            if(result.value == KEY6){       // 6
               right();

            }
            if(result.value == KEY4){       // 4
               left();
            }
            if(result.value == KEY8 ){       // 8
                backward();
            }
            if(result.value == KEY5){       // 5
               stop();
            }
        }
        
    }



    t=clock();
    pc.printf("Hello PCF8574\n");
    while (1) {

        ultra_result = ultra.distance();
        if(ultra_result > 0 && ultra_result < 14) {
             if(in_line[0]==TRU && in_line[1]==TRU ){
                stop(); 
                mode = 2;
                }

            }


        if(mode == 1){
            pc.printf("enter\r\n");
            position = (float)readLine();
            pc.printf("position: %f\r\n", position);
            float proportional =(float)(position-2000.0); //e(t)
            float derivative = (float)(proportional - last_proportional);
            integral += proportional;
            last_proportional = proportional;//
            //pid 15/60000/2 - 85%
            float power_difference = proportional/16.0+ (float)integral/
            60000.0+ derivative*2.0;  //integral 변수 커지고 작아지는 값 확인하
            pc.printf("proportional : %f integral %f derivative : %f \r\n", proportional, integral, derivative);
            const int maximum = 255;
            const int base = 155;
            pc.printf("power_difference is %f\r\n" , power_difference);
            if(power_difference > base ) power_difference = base;
            if(power_difference < -base) power_difference = -base;
            
            
        
            if(power_difference < 0){ //오른쪽으로 빠
    //            rightmotor.setSpeed((float)(base + power_difference) / (float)maximum);
    //            leftmotor.setSpeed((float)base-power_difference/(float)maximum);    
                pc.printf("right    : left %f, right %f \r\n",(float)(base+power_difference) / (float)maximum , (float)(base) / (float)maximum );
                rightmotor.setSpeed((float)(base) / (float)maximum);
                leftmotor.setSpeed((float)(base+power_difference) / (float)maximum );
            } else{// 왼//쪽으로 빠짐.
    //            rightmotor.setSpeed((float)(base + power_difference) / (float)maximum);
    //            leftmotor.setSpeed((float)base-power_difference/(float)maximum); 
                pc.printf("left    : left %f, right %f \r\n",(float)(base) / (float)maximum , (float)(base -power_difference) / (float)maximum );
                    leftmotor.setSpeed((float)(base) / (float)maximum);
                    rightmotor.setSpeed((float)(base -power_difference) / (float)maximum);
                    
                }
                
                if(in_line[0]&&in_line[1]&&in_line[2]&&in_line[3]&&in_line[4]){
                    leftmotor.setSpeed(0);
                    rightmotor.setSpeed(0);
                    pc.printf("stop\r\n");
                    //시간 모니터에 출력.
                    break;
                }         
            }

            if(mode == 2){
                ultra_result = ultra.distance(); 
                                  
                for(int i=0; i<50; i++){
                    data = readLine();
                    if(in_line[2]==TRU && in_line[1]==FALS && in_line[3]==FALS && in_line[0]==FALS && in_line[4]==FALS) 
                    {
                        mode=1;
                        break;
                    }
                    ultraLeft();
                }
            }
    }
                display.begin(true);
                display.setTextSize(1);
                display.setTextColor(WHITE);
                display.setCursor(0,0);
                t=clock()-t;
                sprintf(time, "%f ms", (float)t*10); 
                strcat(displaySentence, time);
                display.println(displaySentence);
                display.display();
}