main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "WS2812.h"
00004 #include "PixelArray.h"
00005 #include "Adafruit_SSD1306.h"
00006 
00007 #include "time.h"
00008 
00009 #define LOW 0
00010 #define HIGH 1
00011 #define KEY2 0x18                 //Key:2 
00012 #define KEY7 0x42                 //Key:2 
00013 #define KEY9 0x4A                 //Key:2 
00014 #define KEY8 0x52                 //Key:8 
00015 #define KEY4 0x08                 //Key:4 
00016 #define KEY6 0x5A                 //Key:6 
00017 #define KEY1 0x0C                 //Key:1 
00018 #define KEY3 0x5E                 //Key:3 
00019 #define KEY5 0x1C                 //Key:5
00020 #define Repeat 0xFF               //press and hold the key
00021 #define Minus 0x07;
00022 #define Plus 0x15;
00023 #define PIN 7
00024 
00025 
00026 #define WS2812_BUF 77   //number of LEDs in the array
00027 #define NUM_COLORS 6    //number of colors to store in the array
00028 #define NUM_STEPS 8    //number of steps between colors
00029 #define I2C_ADDR (0x20)
00030 
00031 //사용 중인 핀 : D4, D5, D6, D7, D9, D10, D11, D12, D13, D14, D15
00032 // A0, A1, A2, A3
00033 PixelArray px(WS2812_BUF);
00034 //WS2812 ws(D7, WS2812_BUF, 0, 5, 5, 0);
00035 WS2812 ws(D7, WS2812_BUF, 6,17,9,14);   //nucleo-f411re
00036 
00037 RawSerial pc(SERIAL_TX, SERIAL_RX,115200);
00038 HCSR04 sensor(D3, D2,pc,0.5);
00039 DigitalIn IR(D4);
00040 InterruptIn remote(D4);
00041 SPI spi(D11, D12, D13);
00042 DigitalOut spi_cs(D10,1);
00043 I2C i2c(I2C_SDA, I2C_SCL);
00044 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);
00045 
00046 
00047 PwmOut PWMA(D6);    //left motor speed
00048 PwmOut PWMB(D5);    //right motor speed
00049 DigitalOut AIN1(A1);    //Mortor-L backward
00050 DigitalOut AIN2(A0);    //Motor-L forward
00051 DigitalOut BIN1(A2);    //Mortor-R forward
00052 DigitalOut BIN2(A3);    //Mortor-R backward
00053 
00054 
00055 int printflag = 0;
00056 int value=0;
00057 int index=0;
00058 int lspeed = 2000;
00059 int rspeed = 2000;
00060 int speed=2000;
00061 int leftcnt = 0;
00062 
00063 int values[] = {0,0,0,0,0};
00064 int whiteMin[]={1000,1000,1000,1000,1000};
00065 int blackMax[]={0,0,0,0,0};
00066 int distancecnt=0;
00067 int stopcnt = 0;
00068 
00069 int driveflag=0;
00070 volatile int flag;
00071 volatile int TRflag=0;
00072 
00073 char IR_decode(unsigned char * code, unsigned char * result1,unsigned char * result2,unsigned char * result3);
00074 
00075 unsigned char results;
00076 unsigned char results1;
00077 unsigned char results2;
00078 unsigned char results3;
00079 
00080 void IR_interrupt();
00081 void translateIR();
00082 
00083 int s=1000;
00084 void forward(int s);
00085 void backward(int s);
00086 void right(int s);
00087 void left(int s);
00088 
00089 void stop();
00090 void setting();
00091 void rx_ISR(void);
00092 void whiteTR();
00093 void blackTR();
00094 
00095 uint8_t rx_buffer[10];
00096 
00097 unsigned long n = 0;
00098 
00099 int color_set(uint8_t red,uint8_t green, uint8_t blue)
00100 {
00101   return ((red<<16) + (green<<8) + blue);   
00102 }
00103 
00104 // 0 <= stepNumber <= lastStepNumber
00105 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)//
00106 {
00107     return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
00108 }
00109 
00110 
00111 int main(void)
00112 {
00113     
00114     int colorIdx = 0;
00115     int colorTo = 0;
00116     int colorFrom = 0;
00117     
00118     uint8_t ir = 0;
00119     uint8_t ig = 0;
00120     uint8_t ib = 0;
00121     
00122     clock_t start, end;
00123     //LED제어/////////////////////////////////////////////////////////////////////////////////////
00124     ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
00125     int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f};
00126 
00127 
00128 
00129     ////////////////////////////////////////////////////////////////////////////////////////////
00130     
00131     sensor.setMode(true);
00132     remote.fall(&IR_interrupt);
00133     setting(); 
00134     pc.attach(&rx_ISR);
00135     spi.format(16,0);
00136     spi.frequency(2000000);
00137     while(driveflag<2)
00138     {
00139         if(results == KEY7 && TRflag ==1)
00140         {
00141             whiteTR();
00142             printf("=======================================================\r\n");
00143             wait_us(21);
00144             driveflag++;
00145 //            gOled2.printf("%d\r\n",driveflag);
00146             pc.printf("%d\r\n",driveflag);
00147         }
00148             
00149         if(results == KEY9 && TRflag ==1)
00150         {
00151             blackTR();
00152             printf("=======================================================\r\n");
00153             wait_us(21);
00154             driveflag++;
00155 //            gOled2.printf("%d\r\n",driveflag);
00156             pc.printf("%d\r\n",driveflag);
00157         }
00158     }
00159     while(driveflag<3){ wait(1);}
00160     int distance;
00161     start=clock();
00162     myOled.printf("\n\n\nRun!");
00163     myOled.display();
00164 
00165     while(1)
00166     {
00167         if(distancecnt%500==1)
00168         {
00169             distance = sensor.distance();
00170             if(distance <20)
00171             {
00172                 leftcnt++;
00173                 if(leftcnt>20)
00174                 {
00175                     right(2300);
00176                     wait(3);
00177                     forward(2300);
00178                 }
00179             }
00180         }
00181         distancecnt++;
00182         for(int i=0; i<6; i++) 
00183         {
00184             //int ch=0;
00185             spi_cs=0;
00186             wait_us(2);
00187             value=spi.write(i<<12);
00188             spi_cs=1;
00189             wait_us(21);
00190             value=value>>6;
00191             if(i>0 && i<6)
00192             {
00193                 values[5-i]=value;
00194                 pc.printf("value%d : %d\r\n",i-1,values[i-1]);
00195             }
00196             
00197         }
00198 
00199 
00200         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])
00201         {
00202             
00203             stopcnt++;
00204             //forward(100);
00205             if(stopcnt>10)
00206             {
00207                 stop();
00208                 pc.printf("stopcnt>10_stop");
00209             }
00210             if(stopcnt>50)
00211             {
00212 
00213                 wait(3);
00214                 stopcnt=0;
00215                 end=clock();
00216                 myOled.printf("\rDriving time: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
00217                 myOled.display();
00218                 pc.printf("시간측정: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
00219                 
00220                 //get starting RGB components for interpolation
00221                 std::size_t c1 = colorbuf[colorFrom];
00222                 std::size_t r1 = (c1 & 0xff0000) >> 16;
00223                 std::size_t g1 = (c1 & 0x00ff00) >> 8;
00224                 std::size_t b1 = (c1 & 0x0000ff);
00225         
00226                 //get ending RGB components for interpolation
00227                 std::size_t c2 = colorbuf[colorTo];
00228                 std::size_t r2 = (c2 & 0xff0000) >> 16;
00229                 std::size_t g2 = (c2 & 0x00ff00) >> 8;
00230                 std::size_t b2 = (c2 & 0x0000ff);
00231     
00232                 for (int i = 0; i <= NUM_STEPS; i++)
00233                 {       
00234                     ir = interpolate(r1, r2, i, NUM_STEPS);
00235                     ig = interpolate(g1, g2, i, NUM_STEPS);
00236                     ib = interpolate(b1, b2, i, NUM_STEPS);
00237             
00238                     //write the color value for each pixel
00239                     px.SetAll(color_set(ir,ig,ib));
00240             
00241                     //write the II value for each pixel
00242                     px.SetAllI(32);
00243             
00244                     for (int i = WS2812_BUF; i >= 0; i--) 
00245                     {
00246                         ws.write(px.getBuf());
00247                     }
00248                 }
00249 //        
00250                 colorFrom = colorIdx;
00251                 colorIdx++;
00252         
00253                 if (colorIdx >= NUM_COLORS)
00254                 {
00255                     colorIdx = 0;
00256                 }
00257             
00258                 colorTo = colorIdx;
00259                 
00260             }
00261         }
00262         else if(values[4]*0.8<blackMax[4])
00263         {
00264             left(1800);
00265             stopcnt=0;
00266             
00267         //---
00268             forward(10);
00269             
00270 //            gOled2.printf("left");
00271           //  pc.printf("left");
00272         }
00273         else if(values[0]*0.8<blackMax[0])
00274         {
00275             right(1800);
00276             stopcnt=0;
00277             //--
00278             forward(10);
00279 //            gOled2.printf("right");
00280           //  pc.printf("right");
00281         }
00282 //        else if(values[1]<blackMax[1] | values[2]<blackMax[2] | values[3]<blackMax[3])
00283 //        {
00284 //            stopcnt=0;
00285 //            forward(2000);
00286 //            pc.printf("fffforward");
00287 //        }
00288         else if(values[2]*0.8<blackMax[2])
00289         {
00290             stopcnt=0;
00291             forward(1800);
00292 //            gOled2.printf("fffforward");
00293            // pc.printf("fffforward");
00294         }
00295         else if(values[1]*0.8<blackMax[1]){
00296             right(600);
00297             stopcnt=0;
00298             //right(300);
00299             forward(1800);
00300             
00301            // pc.printf("r_foward, values1:%d, blackMax1:%d values4:%d blackMax4: %d",values[1],blackMax[1],values[4],blackMax[4]);
00302         }
00303         else if(values[3]*0.85<blackMax[3]){
00304             left(600);
00305             stopcnt=0;
00306             //left(300);
00307             forward(1800);
00308             
00309            // pc.printf("l_forward");
00310         }
00311         else{
00312             stopcnt=0;
00313             forward(100);
00314         }
00315             
00316         
00317     }
00318     
00319 }
00320 
00321 void whiteTR()
00322 {
00323     whiteMin[0]=1000;
00324     whiteMin[1]=1000;
00325     whiteMin[2]=1000;
00326     whiteMin[3]=1000;
00327     whiteMin[4]=1000;
00328     for(int j=0; j<100;j++)
00329     {
00330         for(int i=0; i<7; i++)
00331         {
00332             spi_cs=0;
00333             wait_us(2);
00334             value=spi.write(i<<12);
00335             spi_cs=1;
00336             wait_us(21);
00337             value=value>>6;
00338             if(i>0 && i<6)
00339             {
00340                 if(value<whiteMin[5-i]) whiteMin[5-i]=value/1.4;
00341                 
00342             }
00343         }
00344     }
00345     for(int k=0;k<5;k++)
00346     {
00347 //        gOled2.printf("value%d : %d\r\n",k,whiteMin[k]);
00348         pc.printf("value%d : %d\r\n",k,whiteMin[k]);
00349     }
00350     TRflag=0;
00351 }
00352 
00353 void blackTR()
00354 {
00355     blackMax[0]=0;
00356     blackMax[1]=0;
00357     blackMax[2]=0;
00358     blackMax[3]=0;
00359     blackMax[4]=0;
00360     for(int j=0; j<100;j++)
00361     {
00362         for(int i=0; i<7; i++)
00363         {
00364             spi_cs=0;
00365             wait_us(2);
00366             value=spi.write(i<<12);
00367             spi_cs=1;
00368             wait_us(21);
00369             value=value>>6;
00370             if(i>0 && i<6)
00371             {
00372                 if(value>blackMax[5-i]) blackMax[5-i]=value*1.7;
00373                 
00374             }
00375         }
00376     }
00377     for(int k=0;k<5;k++)
00378     {
00379 //        gOled2.printf("value%d : %d\r\n",k,blackMax[k]);
00380         pc.printf("value%d : %d\r\n",k,blackMax[k]);
00381     }
00382     TRflag=0;
00383 }
00384 
00385 void translateIR() 
00386 {  
00387   switch(results)
00388   {
00389     case KEY1:
00390 
00391       driveflag++;
00392       return;
00393     case KEY2: 
00394       forward(speed);
00395       return;
00396     case KEY3: 
00397       lspeed-=50;
00398       rspeed-=50;
00399       PWMA.pulsewidth_us(lspeed);
00400       PWMB.pulsewidth_us(rspeed);
00401       return;
00402     case KEY4: 
00403       left(1000);
00404       return;
00405     case KEY5: 
00406       stop();
00407       return;
00408     case KEY6: 
00409       right(1000);
00410       return;
00411     case KEY7: 
00412       TRflag=1;
00413       return;
00414     case KEY8: 
00415       backward(speed);
00416       return;
00417     case KEY9: 
00418       TRflag = 1;
00419       return;
00420 
00421     }// End Case
00422 }
00423 
00424 char IR_decode(unsigned char * code, unsigned char * result1,unsigned char * result2,unsigned char * result3)
00425 {
00426   char flag = 0;
00427   unsigned int count = 0;
00428   unsigned char i, index, cnt = 0, data[4] = {0, 0, 0, 0};
00429   if (IR.read() == LOW)
00430   {
00431     count = 0;
00432     while (IR.read() == LOW && count++ < 200)  //9ms
00433       wait_us(60);
00434     count = 0;
00435     while (IR.read() == HIGH && count++ < 80)   //4.5ms
00436       wait_us(60);
00437     for (i = 0; i < 32; i++)
00438     {
00439       count = 0;
00440       while (IR.read() == LOW && count++ < 15) //0.56ms
00441         wait_us(60);
00442       count = 0;
00443       while (IR.read() == HIGH && count++ < 40) //0: 0.56ms; 1: 1.69ms
00444         wait_us(60);
00445       if (count > 20)data[index] |= (1 << cnt);
00446       if (cnt == 7)
00447       {
00448         cnt = 0;
00449         index++;
00450       }
00451       else cnt++;
00452     }
00453     if (data[0] + data[1] == Repeat && data[2] + data[3] == Repeat) //check
00454     {
00455       code[0] = data[2];
00456       result1[0] = data[3];
00457       result2[0] = data[0];
00458       result3[0] = data[1];
00459       printflag=1;
00460       flag = 1;
00461       translateIR();
00462       return flag;
00463     }
00464     if (data[0] == Repeat && data[1] == Repeat && data[2] == Repeat && data[3] == Repeat)
00465     {
00466       code[0] = Repeat;
00467       flag = 1;
00468       return flag;
00469     }
00470   }
00471   return flag;
00472 }
00473 
00474 void forward(int speed)
00475 {
00476   PWMA.pulsewidth_us(speed);
00477   PWMB.pulsewidth_us(speed);
00478   AIN1.write(0);
00479   AIN2.write(1);
00480   BIN1.write(0);
00481   BIN2.write(1);
00482 }
00483 
00484 void backward(int s)
00485 {
00486   PWMA.pulsewidth_us(s);
00487   PWMB.pulsewidth_us(s);
00488   AIN1.write(1);
00489   AIN2.write(0);
00490   BIN1.write(1);
00491   BIN2.write(0);
00492 }
00493 
00494 void right(int s)
00495 {
00496 
00497   PWMA.pulsewidth_us(s);
00498   PWMB.pulsewidth_us(s);
00499   AIN1.write(0);
00500   AIN2.write(1);
00501   BIN1.write(1);
00502   BIN2.write(0);
00503 }
00504 
00505 void left(int s)
00506 {
00507   PWMA.pulsewidth_us(s);
00508   PWMB.pulsewidth_us(s);
00509   AIN1.write(1);
00510   AIN2.write(0);
00511   BIN1.write(0);
00512   BIN2.write(1);
00513 }
00514 
00515 
00516 void stop()
00517 {
00518   PWMA.pulsewidth_us(0);
00519   PWMB.pulsewidth_us(0);
00520   AIN1.write(0);
00521   AIN2.write(0);
00522   BIN1.write(0);
00523   BIN2.write(0);
00524 }
00525 
00526 void setting(){
00527   PWMA.period_ms(10);
00528   PWMB.period_ms(10);
00529 }
00530 
00531 void IR_interrupt()
00532 {
00533     IR_decode(&results, &results1,&results2, &results3);
00534 }
00535 
00536 void rx_ISR(void)
00537 {
00538     char ch;
00539     ch = pc.getc();
00540     pc.putc(ch);
00541     rx_buffer[index++]=ch;
00542     if(ch==0x0D)
00543     {
00544         pc.putc(0x0A);
00545         rx_buffer[--index]='\0';
00546         index=0;
00547         flag=1;
00548     }
00549 }