hgu / Mbed OS final

양예진, 김보희, 홍성은 팀

Dependencies:   Motordriver PixelArray RemoteIR

main.cpp@99:08d58c8231ca, 2019-06-16 (annotated)

Committer:
yangyejin
Date:
Sun Jun 16 23:17:49 2019 +0000
Revision:
99:08d58c8231ca
Parent:
94:9050bb458b00 
q

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yangyejin 94:9050bb458b00 1 #include "mbed.h"
yangyejin 94:9050bb458b00 2 #define button2 18
yangyejin 94:9050bb458b00 3 #include "TB6612FNG.h"
yangyejin 94:9050bb458b00 4 #include "HCSR04.h"
yangyejin 94:9050bb458b00 5 #include "ReceiverIR.h"
yangyejin 94:9050bb458b00 6 #include "TransmitterIR.h"
yangyejin 94:9050bb458b00 7 #include "Adafruit_SSD1306.h" //OLED
yangyejin 94:9050bb458b00 8 #define I2C_ADDR (0x20)
yangyejin 94:9050bb458b00 9
yangyejin 94:9050bb458b00 10 #include "WS28121.h"
yangyejin 94:9050bb458b00 11 #include "PixelArray.h"
yangyejin 94:9050bb458b00 12 #define NUM_LEDS_PER_COLOR 4
yangyejin 94:9050bb458b00 13 #define NUM_COLORS 6 //number of colors to store in the array
yangyejin 94:9050bb458b00 14 #define WS2812_BUF 77 //number of LEDs in the array
yangyejin 94:9050bb458b00 15 uint32_t colors[4]={0xffffff,0xffffff,0xffffff, 0xffffff}; // led1, led2, led3, led4 --> 각 LED color hex 값
yangyejin 94:9050bb458b00 16
yangyejin 94:9050bb458b00 17 PixelArray px(WS2812_BUF);
yangyejin 94:9050bb458b00 18
yangyejin 94:9050bb458b00 19 // See the program page for information on the timing numbers
yangyejin 94:9050bb458b00 20 WS28121 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re
yangyejin 94:9050bb458b00 21
yangyejin 94:9050bb458b00 22 ReceiverIR ir_rx(D4);
yangyejin 94:9050bb458b00 23 Thread sock_thread;
yangyejin 94:9050bb458b00 24 Thread cali_thread;
yangyejin 94:9050bb458b00 25 I2C i2c(I2C_SDA, I2C_SCL);
yangyejin 94:9050bb458b00 26 SPI spi(D11,D12,D13);
yangyejin 94:9050bb458b00 27 DigitalOut spi_cs(D10,1);
yangyejin 94:9050bb458b00 28 RawSerial pc(SERIAL_TX, SERIAL_RX,115200);
yangyejin 94:9050bb458b00 29 TB6612FNG motorDriver(D6, A0, A1, D5, A3, A2, A5);
yangyejin 94:9050bb458b00 30 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);
yangyejin 94:9050bb458b00 31 Thread LED_thread;
yangyejin 94:9050bb458b00 32
yangyejin 94:9050bb458b00 33
yangyejin 94:9050bb458b00 34 void calibrate();
yangyejin 94:9050bb458b00 35 char rx_buffer[10];
yangyejin 94:9050bb458b00 36 int index = 0;
yangyejin 94:9050bb458b00 37 volatile int flag = 0;
yangyejin 94:9050bb458b00 38 Timer t;
yangyejin 94:9050bb458b00 39 int result[6];
yangyejin 94:9050bb458b00 40 bool istart = false;
yangyejin 94:9050bb458b00 41 int calibratedMax[5] = {0,};
yangyejin 94:9050bb458b00 42 int calibratedMin[5] = {1023,};
yangyejin 94:9050bb458b00 43 int sensor_values[5]= {0,0,0,0,0};
yangyejin 94:9050bb458b00 44 int hcsr04();
yangyejin 94:9050bb458b00 45 int start, end;
yangyejin 94:9050bb458b00 46
yangyejin 94:9050bb458b00 47 HCSR04 sensor(D3, D2, pc, 0.5);
yangyejin 94:9050bb458b00 48
yangyejin 94:9050bb458b00 49 int hcsr04(){
yangyejin 94:9050bb458b00 50 int distance;
yangyejin 94:9050bb458b00 51 sensor.setMode(false);
yangyejin 94:9050bb458b00 52 distance = sensor.distance();
yangyejin 94:9050bb458b00 53 sensor.clearStatus();
yangyejin 94:9050bb458b00 54
yangyejin 94:9050bb458b00 55 return distance;
yangyejin 94:9050bb458b00 56 }
yangyejin 94:9050bb458b00 57
yangyejin 94:9050bb458b00 58 void moveStop(void)
yangyejin 94:9050bb458b00 59 {
yangyejin 94:9050bb458b00 60 motorDriver.motorB_stop();
yangyejin 94:9050bb458b00 61 motorDriver.motorA_stop();
yangyejin 94:9050bb458b00 62
yangyejin 94:9050bb458b00 63 }
yangyejin 94:9050bb458b00 64
yangyejin 94:9050bb458b00 65 void moveBackward(float t)
yangyejin 94:9050bb458b00 66 {
yangyejin 94:9050bb458b00 67 motorDriver.motorA_ccw();
yangyejin 94:9050bb458b00 68 motorDriver.motorB_ccw();
yangyejin 94:9050bb458b00 69 //wait(t);
yangyejin 94:9050bb458b00 70 }
yangyejin 94:9050bb458b00 71
yangyejin 94:9050bb458b00 72
yangyejin 94:9050bb458b00 73 void moveForward(float t)
yangyejin 94:9050bb458b00 74 {
yangyejin 94:9050bb458b00 75 motorDriver.motorA_cw();
yangyejin 94:9050bb458b00 76 motorDriver.motorB_cw();
yangyejin 94:9050bb458b00 77 //wait(t); ////
yangyejin 94:9050bb458b00 78 //moveStop();////
yangyejin 94:9050bb458b00 79 }
yangyejin 94:9050bb458b00 80
yangyejin 94:9050bb458b00 81 void moveLeft(float t)
yangyejin 94:9050bb458b00 82 {
yangyejin 94:9050bb458b00 83 motorDriver.motorA_ccw();
yangyejin 94:9050bb458b00 84 motorDriver.motorB_cw();
yangyejin 94:9050bb458b00 85 wait(t);
yangyejin 94:9050bb458b00 86 moveStop();////
yangyejin 94:9050bb458b00 87 }
yangyejin 94:9050bb458b00 88
yangyejin 94:9050bb458b00 89
yangyejin 94:9050bb458b00 90 void moveRight(float t)
yangyejin 94:9050bb458b00 91 {
yangyejin 94:9050bb458b00 92 motorDriver.motorA_cw();
yangyejin 94:9050bb458b00 93 motorDriver.motorB_ccw();
yangyejin 94:9050bb458b00 94 wait(t);
yangyejin 94:9050bb458b00 95 moveStop();////
yangyejin 94:9050bb458b00 96 }
yangyejin 94:9050bb458b00 97
yangyejin 94:9050bb458b00 98
yangyejin 94:9050bb458b00 99 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) {
yangyejin 94:9050bb458b00 100 int cnt = 0;
yangyejin 94:9050bb458b00 101 while (ir_rx.getState() != ReceiverIR::Received) {
yangyejin 94:9050bb458b00 102 cnt++;
yangyejin 94:9050bb458b00 103 if (timeout < cnt) {
yangyejin 94:9050bb458b00 104 return -1;
yangyejin 94:9050bb458b00 105 }
yangyejin 94:9050bb458b00 106 }
yangyejin 94:9050bb458b00 107 return ir_rx.getData(format, buf, bufsiz * 8);
yangyejin 94:9050bb458b00 108 }
yangyejin 94:9050bb458b00 109
yangyejin 94:9050bb458b00 110
yangyejin 94:9050bb458b00 111
yangyejin 94:9050bb458b00 112 /**
yangyejin 94:9050bb458b00 113 * Display a data.
yangyejin 94:9050bb458b00 114 *
yangyejin 94:9050bb458b00 115 * @param buf Pointer to a buffer.
yangyejin 94:9050bb458b00 116 * @param bitlength Bit length of a data.
mbed_official 82:abf1b1785bd7 117 */
mbed_official 82:abf1b1785bd7 118
yangyejin 94:9050bb458b00 119
yangyejin 94:9050bb458b00 120
yangyejin 94:9050bb458b00 121 void set_TLC1543(void)
yangyejin 94:9050bb458b00 122 {
yangyejin 94:9050bb458b00 123 int i;
yangyejin 94:9050bb458b00 124 for(i = 0; i < 6; i++){
yangyejin 94:9050bb458b00 125 int value = 0;
yangyejin 94:9050bb458b00 126 spi_cs = 0;
yangyejin 94:9050bb458b00 127 wait_us(2);
yangyejin 94:9050bb458b00 128 value = spi.write(i<<12);
yangyejin 94:9050bb458b00 129 spi_cs = 1;
yangyejin 94:9050bb458b00 130 wait_us(18);
yangyejin 94:9050bb458b00 131 value = value >> 6;
yangyejin 94:9050bb458b00 132 result[i] = value;
yangyejin 94:9050bb458b00 133 }
yangyejin 94:9050bb458b00 134
yangyejin 94:9050bb458b00 135 for(int i=0; i<5; i++){
yangyejin 94:9050bb458b00 136 sensor_values[i] = result[i+1];
yangyejin 94:9050bb458b00 137 // pc.printf("sensor_values[i] = %d\r\n", i, sensor_values[i]);
yangyejin 94:9050bb458b00 138 }
yangyejin 94:9050bb458b00 139
yangyejin 94:9050bb458b00 140 }
yangyejin 94:9050bb458b00 141
yangyejin 94:9050bb458b00 142 void calibrate()
yangyejin 94:9050bb458b00 143 {
yangyejin 94:9050bb458b00 144 int x;
yangyejin 94:9050bb458b00 145 int max_sensor_values[5] = {0,};
yangyejin 94:9050bb458b00 146 int min_sensor_values[5] = {1024,};
yangyejin 94:9050bb458b00 147
yangyejin 94:9050bb458b00 148 int q;
yangyejin 94:9050bb458b00 149 for(q=0;q<30;q++){
yangyejin 94:9050bb458b00 150 wait(0.5);
yangyejin 94:9050bb458b00 151 printf("count cal : %d\r\n",q);
yangyejin 94:9050bb458b00 152 set_TLC1543();
yangyejin 94:9050bb458b00 153 for(x=0;x<5;x++){
yangyejin 94:9050bb458b00 154 if(q==0 || max_sensor_values[x] < sensor_values[x])
yangyejin 94:9050bb458b00 155 max_sensor_values[x] = sensor_values[x];
yangyejin 94:9050bb458b00 156
yangyejin 94:9050bb458b00 157 if(q==0 || min_sensor_values[x] > sensor_values[x])
yangyejin 94:9050bb458b00 158 min_sensor_values[x] = sensor_values[x];
yangyejin 94:9050bb458b00 159 }
yangyejin 94:9050bb458b00 160
yangyejin 94:9050bb458b00 161 }
yangyejin 94:9050bb458b00 162
yangyejin 94:9050bb458b00 163 for(int i=0; i<5; i++)
yangyejin 94:9050bb458b00 164 {
yangyejin 94:9050bb458b00 165 if(min_sensor_values[i] > calibratedMax[i])
yangyejin 94:9050bb458b00 166 calibratedMax[i] = min_sensor_values[i];
yangyejin 94:9050bb458b00 167 if(max_sensor_values[i] < calibratedMin[i])
yangyejin 94:9050bb458b00 168 calibratedMin[i] = max_sensor_values[i];
yangyejin 94:9050bb458b00 169 }
yangyejin 94:9050bb458b00 170
yangyejin 94:9050bb458b00 171 for(int i = 0 ; i < 5; i++){
yangyejin 94:9050bb458b00 172 pc.printf("cali-Max[%d] : %d\r\n",i,calibratedMax[i]);
yangyejin 94:9050bb458b00 173 pc.printf("cali-Min[%d] : %d\r\n",i,calibratedMin[i]);
yangyejin 94:9050bb458b00 174 }
yangyejin 94:9050bb458b00 175
yangyejin 94:9050bb458b00 176 myOled.printf("End Calibrate\r\n",end-start);
yangyejin 94:9050bb458b00 177 myOled.display();
yangyejin 94:9050bb458b00 178 }
yangyejin 94:9050bb458b00 179
yangyejin 94:9050bb458b00 180 void readCalibrated(int *sensor_values)
yangyejin 94:9050bb458b00 181 {
yangyejin 94:9050bb458b00 182 int i;
yangyejin 94:9050bb458b00 183
yangyejin 94:9050bb458b00 184 set_TLC1543();
yangyejin 94:9050bb458b00 185
yangyejin 94:9050bb458b00 186 for(i=0; i<5; i++)
yangyejin 94:9050bb458b00 187 {
yangyejin 94:9050bb458b00 188 int denominator;
yangyejin 94:9050bb458b00 189
yangyejin 94:9050bb458b00 190 denominator = calibratedMax[i] - calibratedMin[i];
yangyejin 94:9050bb458b00 191
yangyejin 94:9050bb458b00 192 int x=0;
yangyejin 94:9050bb458b00 193 if(denominator != 0)
yangyejin 94:9050bb458b00 194 x = (sensor_values[i] - calibratedMin[i])*1000/denominator;
yangyejin 94:9050bb458b00 195
yangyejin 94:9050bb458b00 196 if(x<0)
yangyejin 94:9050bb458b00 197 x=0;
yangyejin 94:9050bb458b00 198 else if(x>1000)
yangyejin 94:9050bb458b00 199 x=1000;
yangyejin 94:9050bb458b00 200 sensor_values[i] = x;
yangyejin 94:9050bb458b00 201 }
yangyejin 94:9050bb458b00 202 }
Jonathan Austin 0:2757d7abb7d9 203
yangyejin 94:9050bb458b00 204 int readLine(int *sensor_values)
yangyejin 94:9050bb458b00 205 {
yangyejin 94:9050bb458b00 206 char i, on_line = 0;
yangyejin 94:9050bb458b00 207 long avg; // this is for the weighted total, which is long
yangyejin 94:9050bb458b00 208 // before division
yangyejin 94:9050bb458b00 209 int sum; // this is for the denominator which is <= 64000
yangyejin 94:9050bb458b00 210 static int last_value=0; // assume initially that the line is left.
yangyejin 94:9050bb458b00 211
yangyejin 94:9050bb458b00 212 readCalibrated(sensor_values);
yangyejin 94:9050bb458b00 213
yangyejin 94:9050bb458b00 214 avg = 0;
yangyejin 94:9050bb458b00 215 sum = 0;
yangyejin 94:9050bb458b00 216 int _numSensors = 5;
yangyejin 94:9050bb458b00 217
yangyejin 94:9050bb458b00 218 for(i=0;i<_numSensors;i++) {
yangyejin 94:9050bb458b00 219 int value = sensor_values[i];
yangyejin 94:9050bb458b00 220 pc.printf("IR%d : %d\r\n",i,value);
yangyejin 94:9050bb458b00 221
yangyejin 94:9050bb458b00 222
yangyejin 94:9050bb458b00 223 sensor_values[i] = value;
yangyejin 94:9050bb458b00 224 // keep track of whether we see the line at all
yangyejin 94:9050bb458b00 225 if(value > 300) {
yangyejin 94:9050bb458b00 226 on_line = 1;
yangyejin 94:9050bb458b00 227 }
yangyejin 94:9050bb458b00 228
yangyejin 94:9050bb458b00 229 // only average in values that are above a noise threshold
yangyejin 94:9050bb458b00 230 if(value > 50) {
yangyejin 94:9050bb458b00 231 avg += (long)(value) * (i * 1000);
yangyejin 94:9050bb458b00 232 sum += value;
yangyejin 94:9050bb458b00 233 }
yangyejin 94:9050bb458b00 234 }
yangyejin 94:9050bb458b00 235
yangyejin 94:9050bb458b00 236 if(!on_line)
yangyejin 94:9050bb458b00 237 {
yangyejin 94:9050bb458b00 238 // If it last read to the left of center, return 0.
yangyejin 94:9050bb458b00 239 if(last_value < (_numSensors-1)*1000/2)
yangyejin 94:9050bb458b00 240 return 0;
yangyejin 94:9050bb458b00 241
yangyejin 94:9050bb458b00 242 // If it last read to the right of center, return the max.
yangyejin 94:9050bb458b00 243 else
yangyejin 94:9050bb458b00 244 return (_numSensors-1)*1000;
yangyejin 94:9050bb458b00 245 }
yangyejin 94:9050bb458b00 246
yangyejin 94:9050bb458b00 247 last_value = avg/sum;
yangyejin 94:9050bb458b00 248 if(sum > 4500){
yangyejin 94:9050bb458b00 249 last_value = -1;
yangyejin 94:9050bb458b00 250 }
yangyejin 94:9050bb458b00 251
yangyejin 94:9050bb458b00 252 return last_value;
yangyejin 94:9050bb458b00 253 }
yangyejin 94:9050bb458b00 254
yangyejin 94:9050bb458b00 255 void rx_cb(void)
yangyejin 94:9050bb458b00 256 {
yangyejin 94:9050bb458b00 257 char ch;
yangyejin 94:9050bb458b00 258 ch = pc.getc();
yangyejin 94:9050bb458b00 259 pc.putc(ch);
yangyejin 94:9050bb458b00 260 rx_buffer[index++] = ch;
yangyejin 94:9050bb458b00 261 if (ch == 0x0D) { //CR
yangyejin 94:9050bb458b00 262 pc.putc(0x0A); //LF
yangyejin 94:9050bb458b00 263 rx_buffer[--index] = '\0'; //change CR to 0
yangyejin 94:9050bb458b00 264 index = 0;
yangyejin 94:9050bb458b00 265 flag = 1;
yangyejin 94:9050bb458b00 266 }
yangyejin 94:9050bb458b00 267 }
yangyejin 94:9050bb458b00 268
yangyejin 94:9050bb458b00 269 void turnonLED(){
yangyejin 94:9050bb458b00 270 ws.useII(WS28121::PER_PIXEL); // use per-pixel intensity scaling
yangyejin 94:9050bb458b00 271
yangyejin 94:9050bb458b00 272 // set up the colours we want to draw with
yangyejin 94:9050bb458b00 273 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
yangyejin 94:9050bb458b00 274
yangyejin 94:9050bb458b00 275 // for each of the colours (j) write out 10 of them
yangyejin 94:9050bb458b00 276 // the pixels are written at the colour*10, plus the colour position
yangyejin 94:9050bb458b00 277 // all modulus 60 so it wraps around
yangyejin 94:9050bb458b00 278 for (int i = 0; i < WS2812_BUF; i++) {
yangyejin 94:9050bb458b00 279 px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
yangyejin 94:9050bb458b00 280 }
yangyejin 94:9050bb458b00 281
yangyejin 94:9050bb458b00 282 // now all the colours are computed, add a fade effect using intensity scaling
yangyejin 94:9050bb458b00 283 // compute and write the II value for each pixel
yangyejin 94:9050bb458b00 284 for (int j=0; j<WS2812_BUF; j++) {
yangyejin 94:9050bb458b00 285 // px.SetI(pixel position, II value)
yangyejin 94:9050bb458b00 286 px.SetI(j%WS2812_BUF, 0xf+(0xf*(j%NUM_LEDS_PER_COLOR)));
yangyejin 94:9050bb458b00 287 }
yangyejin 94:9050bb458b00 288
yangyejin 94:9050bb458b00 289
yangyejin 94:9050bb458b00 290 // Now the buffer is written, rotate it
yangyejin 94:9050bb458b00 291 // by writing it out with an increasing offset
yangyejin 94:9050bb458b00 292 while (1) {
yangyejin 94:9050bb458b00 293 for (int z=WS2812_BUF; z >= 0 ; z--) {
yangyejin 94:9050bb458b00 294 ws.write_offsets(px.getBuf(),z,z,z);
yangyejin 94:9050bb458b00 295 wait(0.075);
yangyejin 94:9050bb458b00 296 }
yangyejin 94:9050bb458b00 297 }
yangyejin 94:9050bb458b00 298 }
yangyejin 94:9050bb458b00 299
yangyejin 94:9050bb458b00 300 void display_data(uint8_t *buf, int bitlength) {
yangyejin 94:9050bb458b00 301
yangyejin 94:9050bb458b00 302 const int n = bitlength / 8 + (((bitlength % 8) != 0) ? 1 : 0);
Jonathan Austin 0:2757d7abb7d9 303
yangyejin 94:9050bb458b00 304 switch(buf[3]){
yangyejin 94:9050bb458b00 305 case 0xF3: // 1, forward
yangyejin 94:9050bb458b00 306 moveForward(0.2);
yangyejin 94:9050bb458b00 307 wait(0.2);
yangyejin 94:9050bb458b00 308 moveStop();
yangyejin 94:9050bb458b00 309 break;
yangyejin 94:9050bb458b00 310 case 0xE7: //2, stop
yangyejin 94:9050bb458b00 311 moveStop();
yangyejin 94:9050bb458b00 312 break;
yangyejin 94:9050bb458b00 313 case 0xA1: // 3, backward
yangyejin 94:9050bb458b00 314 moveBackward(0.2);
yangyejin 94:9050bb458b00 315 wait(0.2);
yangyejin 94:9050bb458b00 316 moveStop();
yangyejin 94:9050bb458b00 317 break;
yangyejin 94:9050bb458b00 318 case 0xF7: // 4, right
yangyejin 94:9050bb458b00 319 moveRight(0.1);
yangyejin 94:9050bb458b00 320 break;
yangyejin 94:9050bb458b00 321 case 0xE3: // 5, left
yangyejin 94:9050bb458b00 322 moveLeft(0.1);
yangyejin 94:9050bb458b00 323 break;
yangyejin 94:9050bb458b00 324 case 0xBD: // 7
yangyejin 94:9050bb458b00 325 moveForward(0.05);
yangyejin 94:9050bb458b00 326 wait(0.05);
yangyejin 94:9050bb458b00 327 moveStop();
yangyejin 94:9050bb458b00 328 break;
yangyejin 94:9050bb458b00 329 case 0xAD: // 8
yangyejin 94:9050bb458b00 330 moveBackward(0.05);
yangyejin 94:9050bb458b00 331 wait(0.05);
yangyejin 94:9050bb458b00 332 moveStop();
yangyejin 94:9050bb458b00 333 break;
yangyejin 94:9050bb458b00 334 case 0xB5: // 9
yangyejin 94:9050bb458b00 335 motorDriver.motorA_ccw();
yangyejin 94:9050bb458b00 336 motorDriver.motorB_cw();
yangyejin 94:9050bb458b00 337 break;
yangyejin 94:9050bb458b00 338
yangyejin 94:9050bb458b00 339 case 0xBA: // ch-
yangyejin 94:9050bb458b00 340 moveForward(0.1);
yangyejin 94:9050bb458b00 341 wait(0.1);
yangyejin 94:9050bb458b00 342 moveStop();
yangyejin 94:9050bb458b00 343 cali_thread.start(&calibrate);
mbed_official 88:bea4f2daa48c 344
yangyejin 94:9050bb458b00 345 break;
yangyejin 94:9050bb458b00 346
yangyejin 94:9050bb458b00 347 case 0xB9: // ch
yangyejin 94:9050bb458b00 348 pc.printf("start!\r\n");
yangyejin 94:9050bb458b00 349 char data_write[2];
yangyejin 94:9050bb458b00 350 char data_read[2];
yangyejin 94:9050bb458b00 351
yangyejin 94:9050bb458b00 352 data_write[0] = 0xE0;
yangyejin 94:9050bb458b00 353 i2c.write((I2C_ADDR << 1), data_write, 1);
yangyejin 94:9050bb458b00 354 i2c.frequency(100000); // 100 k
yangyejin 94:9050bb458b00 355
yangyejin 94:9050bb458b00 356 t.start();
yangyejin 94:9050bb458b00 357 start = t.read_ms();
yangyejin 94:9050bb458b00 358 while(1) {
yangyejin 94:9050bb458b00 359 pc.printf("--------------------\r\n");
yangyejin 94:9050bb458b00 360 set_TLC1543();
yangyejin 94:9050bb458b00 361 int position = readLine(sensor_values);
yangyejin 94:9050bb458b00 362 pc.printf("position : %d\r\n", position);
yangyejin 94:9050bb458b00 363 int distance = hcsr04();
yangyejin 94:9050bb458b00 364 pc.printf("distance: %d\r\n",distance);
yangyejin 94:9050bb458b00 365 i2c.read(((I2C_ADDR << 1) | 0x01), data_read, 1, 0);
yangyejin 94:9050bb458b00 366 int tempval = data_read[0];
yangyejin 94:9050bb458b00 367 /*
yangyejin 94:9050bb458b00 368 if(tempval == 0x60){
yangyejin 94:9050bb458b00 369 pc.printf("+++++++++left++++++++++\r\n"); //left
yangyejin 94:9050bb458b00 370 moveRight(0.06);
yangyejin 94:9050bb458b00 371 wait(1);
yangyejin 94:9050bb458b00 372 moveForward(0.21 );
yangyejin 94:9050bb458b00 373 wait(1);
yangyejin 94:9050bb458b00 374 moveLeft(0.06);
yangyejin 94:9050bb458b00 375 do{
yangyejin 94:9050bb458b00 376 moveForward(0.06);
yangyejin 94:9050bb458b00 377 wait(0.2);
yangyejin 94:9050bb458b00 378 readCalibrated(sensor_values);
yangyejin 94:9050bb458b00 379 wait(0.3);
yangyejin 94:9050bb458b00 380 }while(sensor_values[3] <700);
yangyejin 94:9050bb458b00 381
yangyejin 94:9050bb458b00 382 continue;
yangyejin 94:9050bb458b00 383 }
yangyejin 94:9050bb458b00 384 else if(tempval == 0xA0){
yangyejin 94:9050bb458b00 385 pc.printf("===========right========\r\n"); //right
yangyejin 94:9050bb458b00 386 moveLeft(0.06);
yangyejin 94:9050bb458b00 387 wait(1);
yangyejin 94:9050bb458b00 388 moveForward(0.22 );
yangyejin 94:9050bb458b00 389 wait(1);
yangyejin 94:9050bb458b00 390 moveRight(0.09);
yangyejin 94:9050bb458b00 391 do{
yangyejin 94:9050bb458b00 392 moveForward(0.06);
yangyejin 94:9050bb458b00 393 wait(0.2);
yangyejin 94:9050bb458b00 394 readCalibrated(sensor_values);
yangyejin 94:9050bb458b00 395 wait(0.3);
yangyejin 94:9050bb458b00 396 }while(sensor_values[3] <700);
yangyejin 94:9050bb458b00 397
yangyejin 94:9050bb458b00 398 continue;
yangyejin 94:9050bb458b00 399 }
yangyejin 94:9050bb458b00 400 */
yangyejin 94:9050bb458b00 401 /*
yangyejin 94:9050bb458b00 402 if(tempval == 0x60){
yangyejin 94:9050bb458b00 403 pc.printf("+++++++++left++++++++++\r\n"); //left
yangyejin 94:9050bb458b00 404 moveRight(0.06);
yangyejin 94:9050bb458b00 405 wait(1);
yangyejin 94:9050bb458b00 406 moveForward(0.24 );
yangyejin 94:9050bb458b00 407 wait(1);
yangyejin 94:9050bb458b00 408 moveLeft(0.06);
yangyejin 94:9050bb458b00 409 do{
yangyejin 94:9050bb458b00 410 moveForward(0.06);
yangyejin 94:9050bb458b00 411 wait(0.2);
yangyejin 94:9050bb458b00 412 readCalibrated(sensor_values);
yangyejin 94:9050bb458b00 413 wait(0.3);
yangyejin 94:9050bb458b00 414 }while(sensor_values[3] <700);
yangyejin 94:9050bb458b00 415
yangyejin 94:9050bb458b00 416 continue;
yangyejin 94:9050bb458b00 417 }
yangyejin 94:9050bb458b00 418 else if(tempval == 0xA0){
yangyejin 94:9050bb458b00 419 pc.printf("===========right========\r\n"); //right
yangyejin 94:9050bb458b00 420 moveLeft(0.06);
yangyejin 94:9050bb458b00 421 wait(1);
yangyejin 94:9050bb458b00 422 moveForward(0.24 );
yangyejin 94:9050bb458b00 423
yangyejin 94:9050bb458b00 424 wait(1);
yangyejin 94:9050bb458b00 425 moveRight(0.09);
yangyejin 94:9050bb458b00 426 do{
yangyejin 94:9050bb458b00 427 moveForward(0.06);
yangyejin 94:9050bb458b00 428 wait(0.2);
yangyejin 94:9050bb458b00 429 readCalibrated(sensor_values);
yangyejin 94:9050bb458b00 430 wait(0.3);
yangyejin 94:9050bb458b00 431 }while(sensor_values[3] <700);
yangyejin 94:9050bb458b00 432
yangyejin 94:9050bb458b00 433 continue;
yangyejin 94:9050bb458b00 434 }
yangyejin 94:9050bb458b00 435
yangyejin 94:9050bb458b00 436 */
yangyejin 94:9050bb458b00 437
yangyejin 94:9050bb458b00 438 if(distance <=21){
yangyejin 94:9050bb458b00 439 pc.printf("stop!!!!please!!\r\n");
yangyejin 94:9050bb458b00 440 moveStop(); ////
yangyejin 94:9050bb458b00 441 moveLeft(0.09);
yangyejin 94:9050bb458b00 442 }
yangyejin 94:9050bb458b00 443
yangyejin 94:9050bb458b00 444 if(position >= 1750 && position < 2600){
yangyejin 94:9050bb458b00 445 moveForward(0.08);
yangyejin 94:9050bb458b00 446 }
yangyejin 94:9050bb458b00 447 else if(position >= 2600 && position < 3000){
yangyejin 94:9050bb458b00 448 moveRight(0.04);
yangyejin 94:9050bb458b00 449 moveStop();////
yangyejin 94:9050bb458b00 450 }
yangyejin 94:9050bb458b00 451 else if(position <= 3500 && position >= 3000){
yangyejin 94:9050bb458b00 452 moveRight(0.03);
yangyejin 94:9050bb458b00 453 }
yangyejin 94:9050bb458b00 454 else if(position <= 4000 && position > 3500){
yangyejin 94:9050bb458b00 455 moveRight(0.02);
yangyejin 94:9050bb458b00 456 }
yangyejin 94:9050bb458b00 457 else if(position < 1750 && position > 1000){
yangyejin 94:9050bb458b00 458 moveLeft(0.04);
yangyejin 94:9050bb458b00 459 }
yangyejin 94:9050bb458b00 460 else if(position <= 1000 && position > 0){
yangyejin 94:9050bb458b00 461 moveLeft(0.03);
yangyejin 94:9050bb458b00 462 }
yangyejin 94:9050bb458b00 463 else if(position == 0){
yangyejin 94:9050bb458b00 464 moveLeft(0.02);
yangyejin 94:9050bb458b00 465 }
yangyejin 94:9050bb458b00 466 else if(position == -1){
yangyejin 94:9050bb458b00 467 moveStop();
yangyejin 94:9050bb458b00 468 end = t.read_ms();
yangyejin 94:9050bb458b00 469 pc.printf("stop!!!!\r\n");
yangyejin 94:9050bb458b00 470 pc.printf("Total time is %d ms.\r\n", end-start);
yangyejin 94:9050bb458b00 471
yangyejin 94:9050bb458b00 472 myOled.printf("Total time is %d ms.\r\n",end-start);
yangyejin 94:9050bb458b00 473 myOled.display();
yangyejin 94:9050bb458b00 474 LED_thread.start(&turnonLED);
yangyejin 94:9050bb458b00 475
yangyejin 94:9050bb458b00 476 wait(5);
yangyejin 94:9050bb458b00 477 }
yangyejin 94:9050bb458b00 478 wait(0.1);
yangyejin 94:9050bb458b00 479 }
yangyejin 94:9050bb458b00 480 break;
yangyejin 94:9050bb458b00 481 }
yangyejin 94:9050bb458b00 482
yangyejin 94:9050bb458b00 483 }
yangyejin 94:9050bb458b00 484 void rx_thread() {
yangyejin 94:9050bb458b00 485 while (1) {
yangyejin 94:9050bb458b00 486 wait(0.2);
yangyejin 94:9050bb458b00 487
yangyejin 94:9050bb458b00 488 uint8_t buf1[32];
yangyejin 94:9050bb458b00 489 int bitlength1;
yangyejin 94:9050bb458b00 490 RemoteIR::Format format;
yangyejin 94:9050bb458b00 491
yangyejin 94:9050bb458b00 492 memset(buf1, 0x00, sizeof(buf1));
yangyejin 94:9050bb458b00 493
yangyejin 94:9050bb458b00 494 bitlength1 = receive(&format, buf1, sizeof(buf1));
yangyejin 94:9050bb458b00 495 if (bitlength1 < 0) {
yangyejin 94:9050bb458b00 496 continue;
yangyejin 94:9050bb458b00 497 }
yangyejin 94:9050bb458b00 498 display_data(buf1, bitlength1);
yangyejin 94:9050bb458b00 499 }
yangyejin 94:9050bb458b00 500 }
yangyejin 94:9050bb458b00 501
mbed_official 82:abf1b1785bd7 502 int main()
mbed_official 82:abf1b1785bd7 503 {
yangyejin 94:9050bb458b00 504 float fPwmPeriod = 0.00002f;//50KHz
yangyejin 94:9050bb458b00 505 float fPwmPulsewidth = 0.250;//duty cycle 0.0~1.0 -> 0%~100%
yangyejin 94:9050bb458b00 506 ///
yangyejin 94:9050bb458b00 507 motorDriver.setPwmAperiod(fPwmPeriod);//set PWM Period
yangyejin 94:9050bb458b00 508 motorDriver.setPwmBperiod(fPwmPeriod);
yangyejin 94:9050bb458b00 509
yangyejin 94:9050bb458b00 510 motorDriver.setPwmApulsewidth(fPwmPulsewidth);//set PWM Puls Width
yangyejin 94:9050bb458b00 511 motorDriver.setPwmBpulsewidth(fPwmPulsewidth);
mbed_official 82:abf1b1785bd7 512
yangyejin 94:9050bb458b00 513 sock_thread.start(&rx_thread);
yangyejin 94:9050bb458b00 514
yangyejin 94:9050bb458b00 515 spi.format(16, 0);
yangyejin 94:9050bb458b00 516 spi.frequency(2000000);
yangyejin 94:9050bb458b00 517
yangyejin 94:9050bb458b00 518 pc.printf("Hello!\r\n");
yangyejin 94:9050bb458b00 519 pc.attach(callback(rx_cb));
yangyejin 94:9050bb458b00 520
yangyejin 94:9050bb458b00 521 //OLED
yangyejin 94:9050bb458b00 522 myOled.begin();
yangyejin 94:9050bb458b00 523 myOled.printf("\nHello World\r\n", myOled.height());
yangyejin 94:9050bb458b00 524 myOled.display();
yangyejin 94:9050bb458b00 525
yangyejin 94:9050bb458b00 526 for(int i = 0; i < 5; i++)
yangyejin 94:9050bb458b00 527 calibratedMin[i] = 1023;
yangyejin 99:08d58c8231ca 528 //fin thread
yangyejin 94:9050bb458b00 529 sock_thread.join();
yangyejin 94:9050bb458b00 530 cali_thread.join();
yangyejin 94:9050bb458b00 531 LED_thread.join();
Jonathan Austin 0:2757d7abb7d9 532 }