양예진, 김보희, 홍성은 팀
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?
User | Revision | Line number | New 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 | } |