main.cpp

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