File content as of revision 1:83eda163ee02:

#include "mbed.h"
#include "hcsr04.h"
#include "WS2812.h"
#include "PixelArray.h"
#include "Adafruit_SSD1306.h"

#include "time.h"

#define LOW 0
#define HIGH 1
#define KEY2 0x18                 //Key:2 
#define KEY7 0x42                 //Key:2 
#define KEY9 0x4A                 //Key:2 
#define KEY8 0x52                 //Key:8 
#define KEY4 0x08                 //Key:4 
#define KEY6 0x5A                 //Key:6 
#define KEY1 0x0C                 //Key:1 
#define KEY3 0x5E                 //Key:3 
#define KEY5 0x1C                 //Key:5
#define Repeat 0xFF               //press and hold the key
#define Minus 0x07;
#define Plus 0x15;
#define PIN 7


#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 I2C_ADDR (0x20)

//사용 중인 핀 : D4, D5, D6, D7, D9, D10, D11, D12, D13, D14, D15
// A0, A1, A2, A3
PixelArray px(WS2812_BUF);
//WS2812 ws(D7, WS2812_BUF, 0, 5, 5, 0);
WS2812 ws(D7, WS2812_BUF, 6,17,9,14);   //nucleo-f411re

RawSerial pc(SERIAL_TX, SERIAL_RX,115200);
HCSR04 sensor(D3, D2,pc,0.5);
DigitalIn IR(D4);
InterruptIn remote(D4);
SPI spi(D11, D12, D13);
DigitalOut spi_cs(D10,1);
I2C i2c(I2C_SDA, I2C_SCL);
Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);


PwmOut PWMA(D6);    //left motor speed
PwmOut PWMB(D5);    //right motor speed
DigitalOut AIN1(A1);    //Mortor-L backward
DigitalOut AIN2(A0);    //Motor-L forward
DigitalOut BIN1(A2);    //Mortor-R forward
DigitalOut BIN2(A3);    //Mortor-R backward


int printflag = 0;
int value=0;
int index=0;
int lspeed = 2000;
int rspeed = 2000;
int speed=2000;
int leftcnt = 0;

int values[] = {0,0,0,0,0};
int whiteMin[]={1000,1000,1000,1000,1000};
int blackMax[]={0,0,0,0,0};
int distancecnt=0;
int stopcnt = 0;

int driveflag=0;
volatile int flag;
volatile int TRflag=0;

char IR_decode(unsigned char * code, unsigned char * result1,unsigned char * result2,unsigned char * result3);

unsigned char results;
unsigned char results1;
unsigned char results2;
unsigned char results3;

void IR_interrupt();
void translateIR();

int s=1000;
void forward(int s);
void backward(int s);
void right(int s);
void left(int s);

void stop();
void setting();
void rx_ISR(void);
void whiteTR();
void blackTR();

uint8_t rx_buffer[10];

unsigned long n = 0;

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


int main(void)
{
    
    int colorIdx = 0;
    int colorTo = 0;
    int colorFrom = 0;
    
    uint8_t ir = 0;
    uint8_t ig = 0;
    uint8_t ib = 0;
    
    clock_t start, end;
    //LED제어/////////////////////////////////////////////////////////////////////////////////////
    ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
    int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f};



    ////////////////////////////////////////////////////////////////////////////////////////////
    
    sensor.setMode(true);
    remote.fall(&IR_interrupt);
    setting(); 
    pc.attach(&rx_ISR);
    spi.format(16,0);
    spi.frequency(2000000);
    while(driveflag<2)
    {
        if(results == KEY7 && TRflag ==1)
        {
            whiteTR();
            printf("=======================================================\r\n");
            wait_us(21);
            driveflag++;
//            gOled2.printf("%d\r\n",driveflag);
            pc.printf("%d\r\n",driveflag);
        }
            
        if(results == KEY9 && TRflag ==1)
        {
            blackTR();
            printf("=======================================================\r\n");
            wait_us(21);
            driveflag++;
//            gOled2.printf("%d\r\n",driveflag);
            pc.printf("%d\r\n",driveflag);
        }
    }
    while(driveflag<3){ wait(1);}
    int distance;
    start=clock();
    myOled.printf("\n\n\nRun!");
    myOled.display();

    while(1)
    {
        if(distancecnt%500==1)
        {
            distance = sensor.distance();
            if(distance <20)
            {
                leftcnt++;
                if(leftcnt>20)
                {
                    right(2300);
                    wait(3);
                    forward(2300);
                }
            }
        }
        distancecnt++;
        for(int i=0; i<6; i++) 
        {
            //int ch=0;
            spi_cs=0;
            wait_us(2);
            value=spi.write(i<<12);
            spi_cs=1;
            wait_us(21);
            value=value>>6;
            if(i>0 && i<6)
            {
                values[5-i]=value;
                pc.printf("value%d : %d\r\n",i-1,values[i-1]);
            }
            
        }


        if(values[0]*0.8<blackMax[0] &&values[1]*0.8<blackMax[1] &&values[2]*0.8<blackMax[2] &&values[3]*0.8<blackMax[3] && values[4]*0.8<blackMax[4])
        {
            
            stopcnt++;
            //forward(100);
            if(stopcnt>10)
            {
                stop();
                pc.printf("stopcnt>10_stop");
            }
            if(stopcnt>50)
            {

                wait(3);
                stopcnt=0;
                end=clock();
                myOled.printf("\rDriving time: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
                myOled.display();
                pc.printf("시간측정: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
                
                //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;
                
            }
        }
        else if(values[4]*0.8<blackMax[4])
        {
            left(1800);
            stopcnt=0;
            
        //---
            forward(10);
            
//            gOled2.printf("left");
          //  pc.printf("left");
        }
        else if(values[0]*0.8<blackMax[0])
        {
            right(1800);
            stopcnt=0;
            //--
            forward(10);
//            gOled2.printf("right");
          //  pc.printf("right");
        }
//        else if(values[1]<blackMax[1] | values[2]<blackMax[2] | values[3]<blackMax[3])
//        {
//            stopcnt=0;
//            forward(2000);
//            pc.printf("fffforward");
//        }
        else if(values[2]*0.8<blackMax[2])
        {
            stopcnt=0;
            forward(1800);
//            gOled2.printf("fffforward");
           // pc.printf("fffforward");
        }
        else if(values[1]*0.8<blackMax[1]){
            right(600);
            stopcnt=0;
            //right(300);
            forward(1800);
            
           // pc.printf("r_foward, values1:%d, blackMax1:%d values4:%d blackMax4: %d",values[1],blackMax[1],values[4],blackMax[4]);
        }
        else if(values[3]*0.85<blackMax[3]){
            left(600);
            stopcnt=0;
            //left(300);
            forward(1800);
            
           // pc.printf("l_forward");
        }
        else{
            stopcnt=0;
            forward(100);
        }
            
        
    }
    
}

void whiteTR()
{
    whiteMin[0]=1000;
    whiteMin[1]=1000;
    whiteMin[2]=1000;
    whiteMin[3]=1000;
    whiteMin[4]=1000;
    for(int j=0; j<100;j++)
    {
        for(int i=0; i<7; i++)
        {
            spi_cs=0;
            wait_us(2);
            value=spi.write(i<<12);
            spi_cs=1;
            wait_us(21);
            value=value>>6;
            if(i>0 && i<6)
            {
                if(value<whiteMin[5-i]) whiteMin[5-i]=value/1.4;
                
            }
        }
    }
    for(int k=0;k<5;k++)
    {
//        gOled2.printf("value%d : %d\r\n",k,whiteMin[k]);
        pc.printf("value%d : %d\r\n",k,whiteMin[k]);
    }
    TRflag=0;
}

void blackTR()
{
    blackMax[0]=0;
    blackMax[1]=0;
    blackMax[2]=0;
    blackMax[3]=0;
    blackMax[4]=0;
    for(int j=0; j<100;j++)
    {
        for(int i=0; i<7; i++)
        {
            spi_cs=0;
            wait_us(2);
            value=spi.write(i<<12);
            spi_cs=1;
            wait_us(21);
            value=value>>6;
            if(i>0 && i<6)
            {
                if(value>blackMax[5-i]) blackMax[5-i]=value*1.7;
                
            }
        }
    }
    for(int k=0;k<5;k++)
    {
//        gOled2.printf("value%d : %d\r\n",k,blackMax[k]);
        pc.printf("value%d : %d\r\n",k,blackMax[k]);
    }
    TRflag=0;
}

void translateIR() 
{  
  switch(results)
  {
    case KEY1:

      driveflag++;
      return;
    case KEY2: 
      forward(speed);
      return;
    case KEY3: 
      lspeed-=50;
      rspeed-=50;
      PWMA.pulsewidth_us(lspeed);
      PWMB.pulsewidth_us(rspeed);
      return;
    case KEY4: 
      left(1000);
      return;
    case KEY5: 
      stop();
      return;
    case KEY6: 
      right(1000);
      return;
    case KEY7: 
      TRflag=1;
      return;
    case KEY8: 
      backward(speed);
      return;
    case KEY9: 
      TRflag = 1;
      return;

    }// End Case
}

char IR_decode(unsigned char * code, unsigned char * result1,unsigned char * result2,unsigned char * result3)
{
  char flag = 0;
  unsigned int count = 0;
  unsigned char i, index, cnt = 0, data[4] = {0, 0, 0, 0};
  if (IR.read() == LOW)
  {
    count = 0;
    while (IR.read() == LOW && count++ < 200)  //9ms
      wait_us(60);
    count = 0;
    while (IR.read() == HIGH && count++ < 80)   //4.5ms
      wait_us(60);
    for (i = 0; i < 32; i++)
    {
      count = 0;
      while (IR.read() == LOW && count++ < 15) //0.56ms
        wait_us(60);
      count = 0;
      while (IR.read() == HIGH && count++ < 40) //0: 0.56ms; 1: 1.69ms
        wait_us(60);
      if (count > 20)data[index] |= (1 << cnt);
      if (cnt == 7)
      {
        cnt = 0;
        index++;
      }
      else cnt++;
    }
    if (data[0] + data[1] == Repeat && data[2] + data[3] == Repeat) //check
    {
      code[0] = data[2];
      result1[0] = data[3];
      result2[0] = data[0];
      result3[0] = data[1];
      printflag=1;
      flag = 1;
      translateIR();
      return flag;
    }
    if (data[0] == Repeat && data[1] == Repeat && data[2] == Repeat && data[3] == Repeat)
    {
      code[0] = Repeat;
      flag = 1;
      return flag;
    }
  }
  return flag;
}

void forward(int speed)
{
  PWMA.pulsewidth_us(speed);
  PWMB.pulsewidth_us(speed);
  AIN1.write(0);
  AIN2.write(1);
  BIN1.write(0);
  BIN2.write(1);
}

void backward(int s)
{
  PWMA.pulsewidth_us(s);
  PWMB.pulsewidth_us(s);
  AIN1.write(1);
  AIN2.write(0);
  BIN1.write(1);
  BIN2.write(0);
}

void right(int s)
{

  PWMA.pulsewidth_us(s);
  PWMB.pulsewidth_us(s);
  AIN1.write(0);
  AIN2.write(1);
  BIN1.write(1);
  BIN2.write(0);
}

void left(int s)
{
  PWMA.pulsewidth_us(s);
  PWMB.pulsewidth_us(s);
  AIN1.write(1);
  AIN2.write(0);
  BIN1.write(0);
  BIN2.write(1);
}


void stop()
{
  PWMA.pulsewidth_us(0);
  PWMB.pulsewidth_us(0);
  AIN1.write(0);
  AIN2.write(0);
  BIN1.write(0);
  BIN2.write(0);
}

void setting(){
  PWMA.period_ms(10);
  PWMB.period_ms(10);
}

void IR_interrupt()
{
    IR_decode(&results, &results1,&results2, &results3);
}

void rx_ISR(void)
{
    char ch;
    ch = pc.getc();
    pc.putc(ch);
    rx_buffer[index++]=ch;
    if(ch==0x0D)
    {
        pc.putc(0x0A);
        rx_buffer[--index]='\0';
        index=0;
        flag=1;
    }
}