Diff: main.cpp

Revision:

0:d5a426cc7a0a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 15 14:02:17 2019 +0000
@@ -0,0 +1,549 @@
+#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;
+    }
+}
\ No newline at end of file