Final Project / Mbed OS Final_copy

finalllll

Dependencies:   WS2812 PixelArray

main.cpp@1:83eda163ee02, 2019-06-15 (annotated)

Committer:
kimhyunjun
Date:
Sat Jun 15 14:50:08 2019 +0000
Revision:
1:83eda163ee02
Parent:
0:d5a426cc7a0a 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kimhyunjun 0:d5a426cc7a0a 1 #include "mbed.h"
kimhyunjun 0:d5a426cc7a0a 2 #include "hcsr04.h"
kimhyunjun 0:d5a426cc7a0a 3 #include "WS2812.h"
kimhyunjun 0:d5a426cc7a0a 4 #include "PixelArray.h"
kimhyunjun 0:d5a426cc7a0a 5 #include "Adafruit_SSD1306.h"
kimhyunjun 0:d5a426cc7a0a 6
kimhyunjun 0:d5a426cc7a0a 7 #include "time.h"
kimhyunjun 0:d5a426cc7a0a 8
kimhyunjun 0:d5a426cc7a0a 9 #define LOW 0
kimhyunjun 0:d5a426cc7a0a 10 #define HIGH 1
kimhyunjun 0:d5a426cc7a0a 11 #define KEY2 0x18 //Key:2
kimhyunjun 0:d5a426cc7a0a 12 #define KEY7 0x42 //Key:2
kimhyunjun 0:d5a426cc7a0a 13 #define KEY9 0x4A //Key:2
kimhyunjun 0:d5a426cc7a0a 14 #define KEY8 0x52 //Key:8
kimhyunjun 0:d5a426cc7a0a 15 #define KEY4 0x08 //Key:4
kimhyunjun 0:d5a426cc7a0a 16 #define KEY6 0x5A //Key:6
kimhyunjun 0:d5a426cc7a0a 17 #define KEY1 0x0C //Key:1
kimhyunjun 0:d5a426cc7a0a 18 #define KEY3 0x5E //Key:3
kimhyunjun 0:d5a426cc7a0a 19 #define KEY5 0x1C //Key:5
kimhyunjun 0:d5a426cc7a0a 20 #define Repeat 0xFF //press and hold the key
kimhyunjun 0:d5a426cc7a0a 21 #define Minus 0x07;
kimhyunjun 0:d5a426cc7a0a 22 #define Plus 0x15;
kimhyunjun 0:d5a426cc7a0a 23 #define PIN 7
kimhyunjun 0:d5a426cc7a0a 24
kimhyunjun 0:d5a426cc7a0a 25
kimhyunjun 0:d5a426cc7a0a 26 #define WS2812_BUF 77 //number of LEDs in the array
kimhyunjun 0:d5a426cc7a0a 27 #define NUM_COLORS 6 //number of colors to store in the array
kimhyunjun 0:d5a426cc7a0a 28 #define NUM_STEPS 8 //number of steps between colors
kimhyunjun 0:d5a426cc7a0a 29 #define I2C_ADDR (0x20)
kimhyunjun 0:d5a426cc7a0a 30
kimhyunjun 0:d5a426cc7a0a 31 //사용 중인 핀 : D4, D5, D6, D7, D9, D10, D11, D12, D13, D14, D15
kimhyunjun 0:d5a426cc7a0a 32 // A0, A1, A2, A3
kimhyunjun 0:d5a426cc7a0a 33 PixelArray px(WS2812_BUF);
kimhyunjun 0:d5a426cc7a0a 34 //WS2812 ws(D7, WS2812_BUF, 0, 5, 5, 0);
kimhyunjun 0:d5a426cc7a0a 35 WS2812 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re
kimhyunjun 0:d5a426cc7a0a 36
kimhyunjun 0:d5a426cc7a0a 37 RawSerial pc(SERIAL_TX, SERIAL_RX,115200);
kimhyunjun 0:d5a426cc7a0a 38 HCSR04 sensor(D3, D2,pc,0.5);
kimhyunjun 0:d5a426cc7a0a 39 DigitalIn IR(D4);
kimhyunjun 0:d5a426cc7a0a 40 InterruptIn remote(D4);
kimhyunjun 0:d5a426cc7a0a 41 SPI spi(D11, D12, D13);
kimhyunjun 0:d5a426cc7a0a 42 DigitalOut spi_cs(D10,1);
kimhyunjun 0:d5a426cc7a0a 43 I2C i2c(I2C_SDA, I2C_SCL);
kimhyunjun 0:d5a426cc7a0a 44 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);
kimhyunjun 0:d5a426cc7a0a 45
kimhyunjun 0:d5a426cc7a0a 46
kimhyunjun 0:d5a426cc7a0a 47 PwmOut PWMA(D6); //left motor speed
kimhyunjun 0:d5a426cc7a0a 48 PwmOut PWMB(D5); //right motor speed
kimhyunjun 0:d5a426cc7a0a 49 DigitalOut AIN1(A1); //Mortor-L backward
kimhyunjun 0:d5a426cc7a0a 50 DigitalOut AIN2(A0); //Motor-L forward
kimhyunjun 0:d5a426cc7a0a 51 DigitalOut BIN1(A2); //Mortor-R forward
kimhyunjun 0:d5a426cc7a0a 52 DigitalOut BIN2(A3); //Mortor-R backward
kimhyunjun 0:d5a426cc7a0a 53
kimhyunjun 0:d5a426cc7a0a 54
kimhyunjun 0:d5a426cc7a0a 55 int printflag = 0;
kimhyunjun 0:d5a426cc7a0a 56 int value=0;
kimhyunjun 0:d5a426cc7a0a 57 int index=0;
kimhyunjun 0:d5a426cc7a0a 58 int lspeed = 2000;
kimhyunjun 0:d5a426cc7a0a 59 int rspeed = 2000;
kimhyunjun 0:d5a426cc7a0a 60 int speed=2000;
kimhyunjun 0:d5a426cc7a0a 61 int leftcnt = 0;
kimhyunjun 0:d5a426cc7a0a 62
kimhyunjun 0:d5a426cc7a0a 63 int values[] = {0,0,0,0,0};
kimhyunjun 0:d5a426cc7a0a 64 int whiteMin[]={1000,1000,1000,1000,1000};
kimhyunjun 0:d5a426cc7a0a 65 int blackMax[]={0,0,0,0,0};
kimhyunjun 0:d5a426cc7a0a 66 int distancecnt=0;
kimhyunjun 0:d5a426cc7a0a 67 int stopcnt = 0;
kimhyunjun 0:d5a426cc7a0a 68
kimhyunjun 0:d5a426cc7a0a 69 int driveflag=0;
kimhyunjun 0:d5a426cc7a0a 70 volatile int flag;
kimhyunjun 0:d5a426cc7a0a 71 volatile int TRflag=0;
kimhyunjun 0:d5a426cc7a0a 72
kimhyunjun 0:d5a426cc7a0a 73 char IR_decode(unsigned char * code, unsigned char * result1,unsigned char * result2,unsigned char * result3);
kimhyunjun 0:d5a426cc7a0a 74
kimhyunjun 0:d5a426cc7a0a 75 unsigned char results;
kimhyunjun 0:d5a426cc7a0a 76 unsigned char results1;
kimhyunjun 0:d5a426cc7a0a 77 unsigned char results2;
kimhyunjun 0:d5a426cc7a0a 78 unsigned char results3;
kimhyunjun 0:d5a426cc7a0a 79
kimhyunjun 0:d5a426cc7a0a 80 void IR_interrupt();
kimhyunjun 0:d5a426cc7a0a 81 void translateIR();
kimhyunjun 0:d5a426cc7a0a 82
kimhyunjun 0:d5a426cc7a0a 83 int s=1000;
kimhyunjun 0:d5a426cc7a0a 84 void forward(int s);
kimhyunjun 0:d5a426cc7a0a 85 void backward(int s);
kimhyunjun 0:d5a426cc7a0a 86 void right(int s);
kimhyunjun 0:d5a426cc7a0a 87 void left(int s);
kimhyunjun 0:d5a426cc7a0a 88
kimhyunjun 0:d5a426cc7a0a 89 void stop();
kimhyunjun 0:d5a426cc7a0a 90 void setting();
kimhyunjun 0:d5a426cc7a0a 91 void rx_ISR(void);
kimhyunjun 0:d5a426cc7a0a 92 void whiteTR();
kimhyunjun 0:d5a426cc7a0a 93 void blackTR();
kimhyunjun 0:d5a426cc7a0a 94
kimhyunjun 0:d5a426cc7a0a 95 uint8_t rx_buffer[10];
kimhyunjun 0:d5a426cc7a0a 96
kimhyunjun 0:d5a426cc7a0a 97 unsigned long n = 0;
kimhyunjun 0:d5a426cc7a0a 98
kimhyunjun 0:d5a426cc7a0a 99 int color_set(uint8_t red,uint8_t green, uint8_t blue)
kimhyunjun 0:d5a426cc7a0a 100 {
kimhyunjun 0:d5a426cc7a0a 101 return ((red<<16) + (green<<8) + blue);
kimhyunjun 0:d5a426cc7a0a 102 }
kimhyunjun 0:d5a426cc7a0a 103
kimhyunjun 0:d5a426cc7a0a 104 // 0 <= stepNumber <= lastStepNumber
kimhyunjun 0:d5a426cc7a0a 105 int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)//
kimhyunjun 0:d5a426cc7a0a 106 {
kimhyunjun 0:d5a426cc7a0a 107 return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
kimhyunjun 0:d5a426cc7a0a 108 }
kimhyunjun 0:d5a426cc7a0a 109
kimhyunjun 0:d5a426cc7a0a 110
kimhyunjun 0:d5a426cc7a0a 111 int main(void)
kimhyunjun 0:d5a426cc7a0a 112 {
kimhyunjun 0:d5a426cc7a0a 113
kimhyunjun 0:d5a426cc7a0a 114 int colorIdx = 0;
kimhyunjun 0:d5a426cc7a0a 115 int colorTo = 0;
kimhyunjun 0:d5a426cc7a0a 116 int colorFrom = 0;
kimhyunjun 0:d5a426cc7a0a 117
kimhyunjun 0:d5a426cc7a0a 118 uint8_t ir = 0;
kimhyunjun 0:d5a426cc7a0a 119 uint8_t ig = 0;
kimhyunjun 0:d5a426cc7a0a 120 uint8_t ib = 0;
kimhyunjun 0:d5a426cc7a0a 121
kimhyunjun 0:d5a426cc7a0a 122 clock_t start, end;
kimhyunjun 0:d5a426cc7a0a 123 //LED제어/////////////////////////////////////////////////////////////////////////////////////
kimhyunjun 0:d5a426cc7a0a 124 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
kimhyunjun 0:d5a426cc7a0a 125 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f};
kimhyunjun 0:d5a426cc7a0a 126
kimhyunjun 0:d5a426cc7a0a 127
kimhyunjun 0:d5a426cc7a0a 128
kimhyunjun 0:d5a426cc7a0a 129 ////////////////////////////////////////////////////////////////////////////////////////////
kimhyunjun 0:d5a426cc7a0a 130
kimhyunjun 0:d5a426cc7a0a 131 sensor.setMode(true);
kimhyunjun 0:d5a426cc7a0a 132 remote.fall(&IR_interrupt);
kimhyunjun 0:d5a426cc7a0a 133 setting();
kimhyunjun 0:d5a426cc7a0a 134 pc.attach(&rx_ISR);
kimhyunjun 0:d5a426cc7a0a 135 spi.format(16,0);
kimhyunjun 0:d5a426cc7a0a 136 spi.frequency(2000000);
kimhyunjun 0:d5a426cc7a0a 137 while(driveflag<2)
kimhyunjun 0:d5a426cc7a0a 138 {
kimhyunjun 0:d5a426cc7a0a 139 if(results == KEY7 && TRflag ==1)
kimhyunjun 0:d5a426cc7a0a 140 {
kimhyunjun 0:d5a426cc7a0a 141 whiteTR();
kimhyunjun 0:d5a426cc7a0a 142 printf("=======================================================\r\n");
kimhyunjun 0:d5a426cc7a0a 143 wait_us(21);
kimhyunjun 0:d5a426cc7a0a 144 driveflag++;
kimhyunjun 0:d5a426cc7a0a 145 // gOled2.printf("%d\r\n",driveflag);
kimhyunjun 0:d5a426cc7a0a 146 pc.printf("%d\r\n",driveflag);
kimhyunjun 0:d5a426cc7a0a 147 }
kimhyunjun 0:d5a426cc7a0a 148
kimhyunjun 0:d5a426cc7a0a 149 if(results == KEY9 && TRflag ==1)
kimhyunjun 0:d5a426cc7a0a 150 {
kimhyunjun 0:d5a426cc7a0a 151 blackTR();
kimhyunjun 0:d5a426cc7a0a 152 printf("=======================================================\r\n");
kimhyunjun 0:d5a426cc7a0a 153 wait_us(21);
kimhyunjun 0:d5a426cc7a0a 154 driveflag++;
kimhyunjun 0:d5a426cc7a0a 155 // gOled2.printf("%d\r\n",driveflag);
kimhyunjun 0:d5a426cc7a0a 156 pc.printf("%d\r\n",driveflag);
kimhyunjun 0:d5a426cc7a0a 157 }
kimhyunjun 0:d5a426cc7a0a 158 }
kimhyunjun 0:d5a426cc7a0a 159 while(driveflag<3){ wait(1);}
kimhyunjun 0:d5a426cc7a0a 160 int distance;
kimhyunjun 0:d5a426cc7a0a 161 start=clock();
kimhyunjun 0:d5a426cc7a0a 162 myOled.printf("\n\n\nRun!");
kimhyunjun 0:d5a426cc7a0a 163 myOled.display();
kimhyunjun 0:d5a426cc7a0a 164
kimhyunjun 0:d5a426cc7a0a 165 while(1)
kimhyunjun 0:d5a426cc7a0a 166 {
kimhyunjun 0:d5a426cc7a0a 167 if(distancecnt%500==1)
kimhyunjun 0:d5a426cc7a0a 168 {
kimhyunjun 0:d5a426cc7a0a 169 distance = sensor.distance();
kimhyunjun 0:d5a426cc7a0a 170 if(distance <20)
kimhyunjun 0:d5a426cc7a0a 171 {
kimhyunjun 0:d5a426cc7a0a 172 leftcnt++;
kimhyunjun 0:d5a426cc7a0a 173 if(leftcnt>20)
kimhyunjun 0:d5a426cc7a0a 174 {
kimhyunjun 0:d5a426cc7a0a 175 right(2300);
kimhyunjun 0:d5a426cc7a0a 176 wait(3);
kimhyunjun 0:d5a426cc7a0a 177 forward(2300);
kimhyunjun 0:d5a426cc7a0a 178 }
kimhyunjun 0:d5a426cc7a0a 179 }
kimhyunjun 0:d5a426cc7a0a 180 }
kimhyunjun 0:d5a426cc7a0a 181 distancecnt++;
kimhyunjun 0:d5a426cc7a0a 182 for(int i=0; i<6; i++)
kimhyunjun 0:d5a426cc7a0a 183 {
kimhyunjun 0:d5a426cc7a0a 184 //int ch=0;
kimhyunjun 0:d5a426cc7a0a 185 spi_cs=0;
kimhyunjun 0:d5a426cc7a0a 186 wait_us(2);
kimhyunjun 0:d5a426cc7a0a 187 value=spi.write(i<<12);
kimhyunjun 0:d5a426cc7a0a 188 spi_cs=1;
kimhyunjun 0:d5a426cc7a0a 189 wait_us(21);
kimhyunjun 0:d5a426cc7a0a 190 value=value>>6;
kimhyunjun 0:d5a426cc7a0a 191 if(i>0 && i<6)
kimhyunjun 0:d5a426cc7a0a 192 {
kimhyunjun 0:d5a426cc7a0a 193 values[5-i]=value;
kimhyunjun 0:d5a426cc7a0a 194 pc.printf("value%d : %d\r\n",i-1,values[i-1]);
kimhyunjun 0:d5a426cc7a0a 195 }
kimhyunjun 0:d5a426cc7a0a 196
kimhyunjun 0:d5a426cc7a0a 197 }
kimhyunjun 0:d5a426cc7a0a 198
kimhyunjun 0:d5a426cc7a0a 199
kimhyunjun 0:d5a426cc7a0a 200 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])
kimhyunjun 0:d5a426cc7a0a 201 {
kimhyunjun 0:d5a426cc7a0a 202
kimhyunjun 0:d5a426cc7a0a 203 stopcnt++;
kimhyunjun 0:d5a426cc7a0a 204 //forward(100);
kimhyunjun 0:d5a426cc7a0a 205 if(stopcnt>10)
kimhyunjun 0:d5a426cc7a0a 206 {
kimhyunjun 0:d5a426cc7a0a 207 stop();
kimhyunjun 0:d5a426cc7a0a 208 pc.printf("stopcnt>10_stop");
kimhyunjun 0:d5a426cc7a0a 209 }
kimhyunjun 0:d5a426cc7a0a 210 if(stopcnt>50)
kimhyunjun 0:d5a426cc7a0a 211 {
kimhyunjun 0:d5a426cc7a0a 212
kimhyunjun 0:d5a426cc7a0a 213 wait(3);
kimhyunjun 0:d5a426cc7a0a 214 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 215 end=clock();
kimhyunjun 0:d5a426cc7a0a 216 myOled.printf("\rDriving time: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
kimhyunjun 0:d5a426cc7a0a 217 myOled.display();
kimhyunjun 0:d5a426cc7a0a 218 pc.printf("시간측정: %.2f",((float)(end-start)/CLOCKS_PER_SEC));
kimhyunjun 0:d5a426cc7a0a 219
kimhyunjun 0:d5a426cc7a0a 220 //get starting RGB components for interpolation
kimhyunjun 0:d5a426cc7a0a 221 std::size_t c1 = colorbuf[colorFrom];
kimhyunjun 0:d5a426cc7a0a 222 std::size_t r1 = (c1 & 0xff0000) >> 16;
kimhyunjun 0:d5a426cc7a0a 223 std::size_t g1 = (c1 & 0x00ff00) >> 8;
kimhyunjun 0:d5a426cc7a0a 224 std::size_t b1 = (c1 & 0x0000ff);
kimhyunjun 0:d5a426cc7a0a 225
kimhyunjun 0:d5a426cc7a0a 226 //get ending RGB components for interpolation
kimhyunjun 0:d5a426cc7a0a 227 std::size_t c2 = colorbuf[colorTo];
kimhyunjun 0:d5a426cc7a0a 228 std::size_t r2 = (c2 & 0xff0000) >> 16;
kimhyunjun 0:d5a426cc7a0a 229 std::size_t g2 = (c2 & 0x00ff00) >> 8;
kimhyunjun 0:d5a426cc7a0a 230 std::size_t b2 = (c2 & 0x0000ff);
kimhyunjun 0:d5a426cc7a0a 231
kimhyunjun 0:d5a426cc7a0a 232 for (int i = 0; i <= NUM_STEPS; i++)
kimhyunjun 0:d5a426cc7a0a 233 {
kimhyunjun 0:d5a426cc7a0a 234 ir = interpolate(r1, r2, i, NUM_STEPS);
kimhyunjun 0:d5a426cc7a0a 235 ig = interpolate(g1, g2, i, NUM_STEPS);
kimhyunjun 0:d5a426cc7a0a 236 ib = interpolate(b1, b2, i, NUM_STEPS);
kimhyunjun 0:d5a426cc7a0a 237
kimhyunjun 0:d5a426cc7a0a 238 //write the color value for each pixel
kimhyunjun 0:d5a426cc7a0a 239 px.SetAll(color_set(ir,ig,ib));
kimhyunjun 0:d5a426cc7a0a 240
kimhyunjun 0:d5a426cc7a0a 241 //write the II value for each pixel
kimhyunjun 0:d5a426cc7a0a 242 px.SetAllI(32);
kimhyunjun 0:d5a426cc7a0a 243
kimhyunjun 0:d5a426cc7a0a 244 for (int i = WS2812_BUF; i >= 0; i--)
kimhyunjun 0:d5a426cc7a0a 245 {
kimhyunjun 0:d5a426cc7a0a 246 ws.write(px.getBuf());
kimhyunjun 0:d5a426cc7a0a 247 }
kimhyunjun 0:d5a426cc7a0a 248 }
kimhyunjun 0:d5a426cc7a0a 249 //
kimhyunjun 0:d5a426cc7a0a 250 colorFrom = colorIdx;
kimhyunjun 0:d5a426cc7a0a 251 colorIdx++;
kimhyunjun 0:d5a426cc7a0a 252
kimhyunjun 0:d5a426cc7a0a 253 if (colorIdx >= NUM_COLORS)
kimhyunjun 0:d5a426cc7a0a 254 {
kimhyunjun 0:d5a426cc7a0a 255 colorIdx = 0;
kimhyunjun 0:d5a426cc7a0a 256 }
kimhyunjun 0:d5a426cc7a0a 257
kimhyunjun 0:d5a426cc7a0a 258 colorTo = colorIdx;
kimhyunjun 0:d5a426cc7a0a 259
kimhyunjun 0:d5a426cc7a0a 260 }
kimhyunjun 0:d5a426cc7a0a 261 }
kimhyunjun 0:d5a426cc7a0a 262 else if(values[4]*0.8<blackMax[4])
kimhyunjun 0:d5a426cc7a0a 263 {
kimhyunjun 0:d5a426cc7a0a 264 left(1800);
kimhyunjun 0:d5a426cc7a0a 265 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 266
kimhyunjun 0:d5a426cc7a0a 267 //---
kimhyunjun 0:d5a426cc7a0a 268 forward(10);
kimhyunjun 0:d5a426cc7a0a 269
kimhyunjun 0:d5a426cc7a0a 270 // gOled2.printf("left");
kimhyunjun 0:d5a426cc7a0a 271 // pc.printf("left");
kimhyunjun 0:d5a426cc7a0a 272 }
kimhyunjun 0:d5a426cc7a0a 273 else if(values[0]*0.8<blackMax[0])
kimhyunjun 0:d5a426cc7a0a 274 {
kimhyunjun 0:d5a426cc7a0a 275 right(1800);
kimhyunjun 0:d5a426cc7a0a 276 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 277 //--
kimhyunjun 0:d5a426cc7a0a 278 forward(10);
kimhyunjun 0:d5a426cc7a0a 279 // gOled2.printf("right");
kimhyunjun 0:d5a426cc7a0a 280 // pc.printf("right");
kimhyunjun 0:d5a426cc7a0a 281 }
kimhyunjun 0:d5a426cc7a0a 282 // else if(values[1]<blackMax[1] | values[2]<blackMax[2] | values[3]<blackMax[3])
kimhyunjun 0:d5a426cc7a0a 283 // {
kimhyunjun 0:d5a426cc7a0a 284 // stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 285 // forward(2000);
kimhyunjun 0:d5a426cc7a0a 286 // pc.printf("fffforward");
kimhyunjun 0:d5a426cc7a0a 287 // }
kimhyunjun 0:d5a426cc7a0a 288 else if(values[2]*0.8<blackMax[2])
kimhyunjun 0:d5a426cc7a0a 289 {
kimhyunjun 0:d5a426cc7a0a 290 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 291 forward(1800);
kimhyunjun 0:d5a426cc7a0a 292 // gOled2.printf("fffforward");
kimhyunjun 0:d5a426cc7a0a 293 // pc.printf("fffforward");
kimhyunjun 0:d5a426cc7a0a 294 }
kimhyunjun 0:d5a426cc7a0a 295 else if(values[1]*0.8<blackMax[1]){
kimhyunjun 0:d5a426cc7a0a 296 right(600);
kimhyunjun 0:d5a426cc7a0a 297 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 298 //right(300);
kimhyunjun 0:d5a426cc7a0a 299 forward(1800);
kimhyunjun 0:d5a426cc7a0a 300
kimhyunjun 0:d5a426cc7a0a 301 // pc.printf("r_foward, values1:%d, blackMax1:%d values4:%d blackMax4: %d",values[1],blackMax[1],values[4],blackMax[4]);
kimhyunjun 0:d5a426cc7a0a 302 }
kimhyunjun 0:d5a426cc7a0a 303 else if(values[3]*0.85<blackMax[3]){
kimhyunjun 0:d5a426cc7a0a 304 left(600);
kimhyunjun 0:d5a426cc7a0a 305 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 306 //left(300);
kimhyunjun 0:d5a426cc7a0a 307 forward(1800);
kimhyunjun 0:d5a426cc7a0a 308
kimhyunjun 0:d5a426cc7a0a 309 // pc.printf("l_forward");
kimhyunjun 0:d5a426cc7a0a 310 }
kimhyunjun 0:d5a426cc7a0a 311 else{
kimhyunjun 0:d5a426cc7a0a 312 stopcnt=0;
kimhyunjun 0:d5a426cc7a0a 313 forward(100);
kimhyunjun 0:d5a426cc7a0a 314 }
kimhyunjun 0:d5a426cc7a0a 315
kimhyunjun 0:d5a426cc7a0a 316
kimhyunjun 0:d5a426cc7a0a 317 }
kimhyunjun 0:d5a426cc7a0a 318
kimhyunjun 0:d5a426cc7a0a 319 }
kimhyunjun 0:d5a426cc7a0a 320
kimhyunjun 0:d5a426cc7a0a 321 void whiteTR()
kimhyunjun 0:d5a426cc7a0a 322 {
kimhyunjun 0:d5a426cc7a0a 323 whiteMin[0]=1000;
kimhyunjun 0:d5a426cc7a0a 324 whiteMin[1]=1000;
kimhyunjun 0:d5a426cc7a0a 325 whiteMin[2]=1000;
kimhyunjun 0:d5a426cc7a0a 326 whiteMin[3]=1000;
kimhyunjun 0:d5a426cc7a0a 327 whiteMin[4]=1000;
kimhyunjun 0:d5a426cc7a0a 328 for(int j=0; j<100;j++)
kimhyunjun 0:d5a426cc7a0a 329 {
kimhyunjun 0:d5a426cc7a0a 330 for(int i=0; i<7; i++)
kimhyunjun 0:d5a426cc7a0a 331 {
kimhyunjun 0:d5a426cc7a0a 332 spi_cs=0;
kimhyunjun 0:d5a426cc7a0a 333 wait_us(2);
kimhyunjun 0:d5a426cc7a0a 334 value=spi.write(i<<12);
kimhyunjun 0:d5a426cc7a0a 335 spi_cs=1;
kimhyunjun 0:d5a426cc7a0a 336 wait_us(21);
kimhyunjun 0:d5a426cc7a0a 337 value=value>>6;
kimhyunjun 0:d5a426cc7a0a 338 if(i>0 && i<6)
kimhyunjun 0:d5a426cc7a0a 339 {
kimhyunjun 0:d5a426cc7a0a 340 if(value<whiteMin[5-i]) whiteMin[5-i]=value/1.4;
kimhyunjun 0:d5a426cc7a0a 341
kimhyunjun 0:d5a426cc7a0a 342 }
kimhyunjun 0:d5a426cc7a0a 343 }
kimhyunjun 0:d5a426cc7a0a 344 }
kimhyunjun 0:d5a426cc7a0a 345 for(int k=0;k<5;k++)
kimhyunjun 0:d5a426cc7a0a 346 {
kimhyunjun 0:d5a426cc7a0a 347 // gOled2.printf("value%d : %d\r\n",k,whiteMin[k]);
kimhyunjun 0:d5a426cc7a0a 348 pc.printf("value%d : %d\r\n",k,whiteMin[k]);
kimhyunjun 0:d5a426cc7a0a 349 }
kimhyunjun 0:d5a426cc7a0a 350 TRflag=0;
kimhyunjun 0:d5a426cc7a0a 351 }
kimhyunjun 0:d5a426cc7a0a 352
kimhyunjun 0:d5a426cc7a0a 353 void blackTR()
kimhyunjun 0:d5a426cc7a0a 354 {
kimhyunjun 0:d5a426cc7a0a 355 blackMax[0]=0;
kimhyunjun 0:d5a426cc7a0a 356 blackMax[1]=0;
kimhyunjun 0:d5a426cc7a0a 357 blackMax[2]=0;
kimhyunjun 0:d5a426cc7a0a 358 blackMax[3]=0;
kimhyunjun 0:d5a426cc7a0a 359 blackMax[4]=0;
kimhyunjun 0:d5a426cc7a0a 360 for(int j=0; j<100;j++)
kimhyunjun 0:d5a426cc7a0a 361 {
kimhyunjun 0:d5a426cc7a0a 362 for(int i=0; i<7; i++)
kimhyunjun 0:d5a426cc7a0a 363 {
kimhyunjun 0:d5a426cc7a0a 364 spi_cs=0;
kimhyunjun 0:d5a426cc7a0a 365 wait_us(2);
kimhyunjun 0:d5a426cc7a0a 366 value=spi.write(i<<12);
kimhyunjun 0:d5a426cc7a0a 367 spi_cs=1;
kimhyunjun 0:d5a426cc7a0a 368 wait_us(21);
kimhyunjun 0:d5a426cc7a0a 369 value=value>>6;
kimhyunjun 0:d5a426cc7a0a 370 if(i>0 && i<6)
kimhyunjun 0:d5a426cc7a0a 371 {
kimhyunjun 0:d5a426cc7a0a 372 if(value>blackMax[5-i]) blackMax[5-i]=value*1.7;
kimhyunjun 0:d5a426cc7a0a 373
kimhyunjun 0:d5a426cc7a0a 374 }
kimhyunjun 0:d5a426cc7a0a 375 }
kimhyunjun 0:d5a426cc7a0a 376 }
kimhyunjun 0:d5a426cc7a0a 377 for(int k=0;k<5;k++)
kimhyunjun 0:d5a426cc7a0a 378 {
kimhyunjun 0:d5a426cc7a0a 379 // gOled2.printf("value%d : %d\r\n",k,blackMax[k]);
kimhyunjun 0:d5a426cc7a0a 380 pc.printf("value%d : %d\r\n",k,blackMax[k]);
kimhyunjun 0:d5a426cc7a0a 381 }
kimhyunjun 0:d5a426cc7a0a 382 TRflag=0;
kimhyunjun 0:d5a426cc7a0a 383 }
kimhyunjun 0:d5a426cc7a0a 384
kimhyunjun 0:d5a426cc7a0a 385 void translateIR()
kimhyunjun 0:d5a426cc7a0a 386 {
kimhyunjun 0:d5a426cc7a0a 387 switch(results)
kimhyunjun 0:d5a426cc7a0a 388 {
kimhyunjun 0:d5a426cc7a0a 389 case KEY1:
kimhyunjun 0:d5a426cc7a0a 390
kimhyunjun 0:d5a426cc7a0a 391 driveflag++;
kimhyunjun 0:d5a426cc7a0a 392 return;
kimhyunjun 0:d5a426cc7a0a 393 case KEY2:
kimhyunjun 0:d5a426cc7a0a 394 forward(speed);
kimhyunjun 0:d5a426cc7a0a 395 return;
kimhyunjun 0:d5a426cc7a0a 396 case KEY3:
kimhyunjun 0:d5a426cc7a0a 397 lspeed-=50;
kimhyunjun 0:d5a426cc7a0a 398 rspeed-=50;
kimhyunjun 0:d5a426cc7a0a 399 PWMA.pulsewidth_us(lspeed);
kimhyunjun 0:d5a426cc7a0a 400 PWMB.pulsewidth_us(rspeed);
kimhyunjun 0:d5a426cc7a0a 401 return;
kimhyunjun 0:d5a426cc7a0a 402 case KEY4:
kimhyunjun 0:d5a426cc7a0a 403 left(1000);
kimhyunjun 0:d5a426cc7a0a 404 return;
kimhyunjun 0:d5a426cc7a0a 405 case KEY5:
kimhyunjun 0:d5a426cc7a0a 406 stop();
kimhyunjun 0:d5a426cc7a0a 407 return;
kimhyunjun 0:d5a426cc7a0a 408 case KEY6:
kimhyunjun 0:d5a426cc7a0a 409 right(1000);
kimhyunjun 0:d5a426cc7a0a 410 return;
kimhyunjun 0:d5a426cc7a0a 411 case KEY7:
kimhyunjun 0:d5a426cc7a0a 412 TRflag=1;
kimhyunjun 0:d5a426cc7a0a 413 return;
kimhyunjun 0:d5a426cc7a0a 414 case KEY8:
kimhyunjun 0:d5a426cc7a0a 415 backward(speed);
kimhyunjun 0:d5a426cc7a0a 416 return;
kimhyunjun 0:d5a426cc7a0a 417 case KEY9:
kimhyunjun 0:d5a426cc7a0a 418 TRflag = 1;
kimhyunjun 0:d5a426cc7a0a 419 return;
kimhyunjun 0:d5a426cc7a0a 420
kimhyunjun 0:d5a426cc7a0a 421 }// End Case
kimhyunjun 0:d5a426cc7a0a 422 }
kimhyunjun 0:d5a426cc7a0a 423
kimhyunjun 0:d5a426cc7a0a 424 char IR_decode(unsigned char * code, unsigned char * result1,unsigned char * result2,unsigned char * result3)
kimhyunjun 0:d5a426cc7a0a 425 {
kimhyunjun 0:d5a426cc7a0a 426 char flag = 0;
kimhyunjun 0:d5a426cc7a0a 427 unsigned int count = 0;
kimhyunjun 0:d5a426cc7a0a 428 unsigned char i, index, cnt = 0, data[4] = {0, 0, 0, 0};
kimhyunjun 0:d5a426cc7a0a 429 if (IR.read() == LOW)
kimhyunjun 0:d5a426cc7a0a 430 {
kimhyunjun 0:d5a426cc7a0a 431 count = 0;
kimhyunjun 0:d5a426cc7a0a 432 while (IR.read() == LOW && count++ < 200) //9ms
kimhyunjun 0:d5a426cc7a0a 433 wait_us(60);
kimhyunjun 0:d5a426cc7a0a 434 count = 0;
kimhyunjun 0:d5a426cc7a0a 435 while (IR.read() == HIGH && count++ < 80) //4.5ms
kimhyunjun 0:d5a426cc7a0a 436 wait_us(60);
kimhyunjun 0:d5a426cc7a0a 437 for (i = 0; i < 32; i++)
kimhyunjun 0:d5a426cc7a0a 438 {
kimhyunjun 0:d5a426cc7a0a 439 count = 0;
kimhyunjun 0:d5a426cc7a0a 440 while (IR.read() == LOW && count++ < 15) //0.56ms
kimhyunjun 0:d5a426cc7a0a 441 wait_us(60);
kimhyunjun 0:d5a426cc7a0a 442 count = 0;
kimhyunjun 0:d5a426cc7a0a 443 while (IR.read() == HIGH && count++ < 40) //0: 0.56ms; 1: 1.69ms
kimhyunjun 0:d5a426cc7a0a 444 wait_us(60);
kimhyunjun 0:d5a426cc7a0a 445 if (count > 20)data[index] |= (1 << cnt);
kimhyunjun 0:d5a426cc7a0a 446 if (cnt == 7)
kimhyunjun 0:d5a426cc7a0a 447 {
kimhyunjun 0:d5a426cc7a0a 448 cnt = 0;
kimhyunjun 0:d5a426cc7a0a 449 index++;
kimhyunjun 0:d5a426cc7a0a 450 }
kimhyunjun 0:d5a426cc7a0a 451 else cnt++;
kimhyunjun 0:d5a426cc7a0a 452 }
kimhyunjun 0:d5a426cc7a0a 453 if (data[0] + data[1] == Repeat && data[2] + data[3] == Repeat) //check
kimhyunjun 0:d5a426cc7a0a 454 {
kimhyunjun 0:d5a426cc7a0a 455 code[0] = data[2];
kimhyunjun 0:d5a426cc7a0a 456 result1[0] = data[3];
kimhyunjun 0:d5a426cc7a0a 457 result2[0] = data[0];
kimhyunjun 0:d5a426cc7a0a 458 result3[0] = data[1];
kimhyunjun 0:d5a426cc7a0a 459 printflag=1;
kimhyunjun 0:d5a426cc7a0a 460 flag = 1;
kimhyunjun 0:d5a426cc7a0a 461 translateIR();
kimhyunjun 0:d5a426cc7a0a 462 return flag;
kimhyunjun 0:d5a426cc7a0a 463 }
kimhyunjun 0:d5a426cc7a0a 464 if (data[0] == Repeat && data[1] == Repeat && data[2] == Repeat && data[3] == Repeat)
kimhyunjun 0:d5a426cc7a0a 465 {
kimhyunjun 0:d5a426cc7a0a 466 code[0] = Repeat;
kimhyunjun 0:d5a426cc7a0a 467 flag = 1;
kimhyunjun 0:d5a426cc7a0a 468 return flag;
kimhyunjun 0:d5a426cc7a0a 469 }
kimhyunjun 0:d5a426cc7a0a 470 }
kimhyunjun 0:d5a426cc7a0a 471 return flag;
kimhyunjun 0:d5a426cc7a0a 472 }
kimhyunjun 0:d5a426cc7a0a 473
kimhyunjun 0:d5a426cc7a0a 474 void forward(int speed)
kimhyunjun 0:d5a426cc7a0a 475 {
kimhyunjun 0:d5a426cc7a0a 476 PWMA.pulsewidth_us(speed);
kimhyunjun 0:d5a426cc7a0a 477 PWMB.pulsewidth_us(speed);
kimhyunjun 0:d5a426cc7a0a 478 AIN1.write(0);
kimhyunjun 0:d5a426cc7a0a 479 AIN2.write(1);
kimhyunjun 0:d5a426cc7a0a 480 BIN1.write(0);
kimhyunjun 0:d5a426cc7a0a 481 BIN2.write(1);
kimhyunjun 0:d5a426cc7a0a 482 }
kimhyunjun 0:d5a426cc7a0a 483
kimhyunjun 0:d5a426cc7a0a 484 void backward(int s)
kimhyunjun 0:d5a426cc7a0a 485 {
kimhyunjun 0:d5a426cc7a0a 486 PWMA.pulsewidth_us(s);
kimhyunjun 0:d5a426cc7a0a 487 PWMB.pulsewidth_us(s);
kimhyunjun 0:d5a426cc7a0a 488 AIN1.write(1);
kimhyunjun 0:d5a426cc7a0a 489 AIN2.write(0);
kimhyunjun 0:d5a426cc7a0a 490 BIN1.write(1);
kimhyunjun 0:d5a426cc7a0a 491 BIN2.write(0);
kimhyunjun 0:d5a426cc7a0a 492 }
kimhyunjun 0:d5a426cc7a0a 493
kimhyunjun 0:d5a426cc7a0a 494 void right(int s)
kimhyunjun 0:d5a426cc7a0a 495 {
kimhyunjun 0:d5a426cc7a0a 496
kimhyunjun 0:d5a426cc7a0a 497 PWMA.pulsewidth_us(s);
kimhyunjun 0:d5a426cc7a0a 498 PWMB.pulsewidth_us(s);
kimhyunjun 0:d5a426cc7a0a 499 AIN1.write(0);
kimhyunjun 0:d5a426cc7a0a 500 AIN2.write(1);
kimhyunjun 0:d5a426cc7a0a 501 BIN1.write(1);
kimhyunjun 0:d5a426cc7a0a 502 BIN2.write(0);
kimhyunjun 0:d5a426cc7a0a 503 }
kimhyunjun 0:d5a426cc7a0a 504
kimhyunjun 0:d5a426cc7a0a 505 void left(int s)
kimhyunjun 0:d5a426cc7a0a 506 {
kimhyunjun 0:d5a426cc7a0a 507 PWMA.pulsewidth_us(s);
kimhyunjun 0:d5a426cc7a0a 508 PWMB.pulsewidth_us(s);
kimhyunjun 0:d5a426cc7a0a 509 AIN1.write(1);
kimhyunjun 0:d5a426cc7a0a 510 AIN2.write(0);
kimhyunjun 0:d5a426cc7a0a 511 BIN1.write(0);
kimhyunjun 0:d5a426cc7a0a 512 BIN2.write(1);
kimhyunjun 0:d5a426cc7a0a 513 }
kimhyunjun 0:d5a426cc7a0a 514
kimhyunjun 0:d5a426cc7a0a 515
kimhyunjun 0:d5a426cc7a0a 516 void stop()
kimhyunjun 0:d5a426cc7a0a 517 {
kimhyunjun 0:d5a426cc7a0a 518 PWMA.pulsewidth_us(0);
kimhyunjun 0:d5a426cc7a0a 519 PWMB.pulsewidth_us(0);
kimhyunjun 0:d5a426cc7a0a 520 AIN1.write(0);
kimhyunjun 0:d5a426cc7a0a 521 AIN2.write(0);
kimhyunjun 0:d5a426cc7a0a 522 BIN1.write(0);
kimhyunjun 0:d5a426cc7a0a 523 BIN2.write(0);
kimhyunjun 0:d5a426cc7a0a 524 }
kimhyunjun 0:d5a426cc7a0a 525
kimhyunjun 0:d5a426cc7a0a 526 void setting(){
kimhyunjun 0:d5a426cc7a0a 527 PWMA.period_ms(10);
kimhyunjun 0:d5a426cc7a0a 528 PWMB.period_ms(10);
kimhyunjun 0:d5a426cc7a0a 529 }
kimhyunjun 0:d5a426cc7a0a 530
kimhyunjun 0:d5a426cc7a0a 531 void IR_interrupt()
kimhyunjun 0:d5a426cc7a0a 532 {
kimhyunjun 0:d5a426cc7a0a 533 IR_decode(&results, &results1,&results2, &results3);
kimhyunjun 0:d5a426cc7a0a 534 }
kimhyunjun 0:d5a426cc7a0a 535
kimhyunjun 0:d5a426cc7a0a 536 void rx_ISR(void)
kimhyunjun 0:d5a426cc7a0a 537 {
kimhyunjun 0:d5a426cc7a0a 538 char ch;
kimhyunjun 0:d5a426cc7a0a 539 ch = pc.getc();
kimhyunjun 0:d5a426cc7a0a 540 pc.putc(ch);
kimhyunjun 0:d5a426cc7a0a 541 rx_buffer[index++]=ch;
kimhyunjun 0:d5a426cc7a0a 542 if(ch==0x0D)
kimhyunjun 0:d5a426cc7a0a 543 {
kimhyunjun 0:d5a426cc7a0a 544 pc.putc(0x0A);
kimhyunjun 0:d5a426cc7a0a 545 rx_buffer[--index]='\0';
kimhyunjun 0:d5a426cc7a0a 546 index=0;
kimhyunjun 0:d5a426cc7a0a 547 flag=1;
kimhyunjun 0:d5a426cc7a0a 548 }
kimhyunjun 0:d5a426cc7a0a 549 }