Final Project
zz
Dependencies: WS2812 PixelArray final_project
Diff: main.cpp
- Revision:
- 0:c96221a59771
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Jun 16 17:35:35 2019 +0000
@@ -0,0 +1,464 @@
+#include "mbed.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
+
+//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 oledflag=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 exitcnt =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();
+
+
+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();
+ spi.format(16,0);
+ spi.frequency(2000000);
+ while(driveflag<2)
+ {
+ if(results == KEY7 && TRflag ==1)
+ {
+ whiteTR();
+ printf("=======================================================\r\n");
+ wait_us(21);
+ driveflag++;
+
+ }
+
+ if(results == KEY9 && TRflag ==1)
+ {
+ blackTR();
+ printf("=======================================================\r\n");
+ wait_us(21);
+ driveflag++;
+
+ }
+ }
+ while(driveflag<3){ wait(1);}
+ start=clock();
+ myOled.printf("\n\n\nRun!");
+ myOled.display();
+
+ while(1)
+ {
+ for(int i=0; i<6; 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)
+ {
+ values[5-i]=value;
+ }
+
+ }
+
+
+ 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])
+ {
+ backward(1800);
+ forward(2300);
+ stopcnt++;
+ if(stopcnt>20)
+ {
+ stop();
+ wait(3);
+ end=clock();
+ if(oledflag==0)
+ {
+ myOled.printf("\rDriving time: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
+ myOled.display();
+ oledflag=1;
+ }
+ break;
+ }
+ }
+ else if(values[4]*0.7<blackMax[4] && values[0]*1.2>whiteMin[0])
+ {
+ stopcnt=0;
+ forward(2300);
+ left(2300);
+ }
+ else if(values[0]*0.7<blackMax[0]&& values[4]*1.2>whiteMin[4])
+ {
+ stopcnt=0;
+ forward(2300);
+ right(2300);
+
+ }
+
+ else{
+ stopcnt=0;
+ forward(2300);
+ }
+
+
+ }
+ while(1)
+ {
+ 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;
+ wait(1);
+ }
+}
+
+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;
+
+ }
+ }
+ }
+ 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;
+
+ }
+ }
+ }
+ TRflag=0;
+}
+
+void translateIR()
+{
+ switch(results)
+ {
+ case KEY1:
+ driveflag++;
+ return;
+ case KEY2:
+ forward(1500);
+ 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(1500);
+ 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);
+}
\ No newline at end of file