양예진, 김보희, 홍성은 팀
Dependencies: Motordriver PixelArray RemoteIR
main.cpp
- Committer:
- yangyejin
- Date:
- 2019-06-16
- Revision:
- 99:08d58c8231ca
- Parent:
- 94:9050bb458b00
File content as of revision 99:08d58c8231ca:
#include "mbed.h" #define button2 18 #include "TB6612FNG.h" #include "HCSR04.h" #include "ReceiverIR.h" #include "TransmitterIR.h" #include "Adafruit_SSD1306.h" //OLED #define I2C_ADDR (0x20) #include "WS28121.h" #include "PixelArray.h" #define NUM_LEDS_PER_COLOR 4 #define NUM_COLORS 6 //number of colors to store in the array #define WS2812_BUF 77 //number of LEDs in the array uint32_t colors[4]={0xffffff,0xffffff,0xffffff, 0xffffff}; // led1, led2, led3, led4 --> 각 LED color hex 값 PixelArray px(WS2812_BUF); // See the program page for information on the timing numbers WS28121 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re ReceiverIR ir_rx(D4); Thread sock_thread; Thread cali_thread; I2C i2c(I2C_SDA, I2C_SCL); SPI spi(D11,D12,D13); DigitalOut spi_cs(D10,1); RawSerial pc(SERIAL_TX, SERIAL_RX,115200); TB6612FNG motorDriver(D6, A0, A1, D5, A3, A2, A5); Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); Thread LED_thread; void calibrate(); char rx_buffer[10]; int index = 0; volatile int flag = 0; Timer t; int result[6]; bool istart = false; int calibratedMax[5] = {0,}; int calibratedMin[5] = {1023,}; int sensor_values[5]= {0,0,0,0,0}; int hcsr04(); int start, end; HCSR04 sensor(D3, D2, pc, 0.5); int hcsr04(){ int distance; sensor.setMode(false); distance = sensor.distance(); sensor.clearStatus(); return distance; } void moveStop(void) { motorDriver.motorB_stop(); motorDriver.motorA_stop(); } void moveBackward(float t) { motorDriver.motorA_ccw(); motorDriver.motorB_ccw(); //wait(t); } void moveForward(float t) { motorDriver.motorA_cw(); motorDriver.motorB_cw(); //wait(t); //// //moveStop();//// } void moveLeft(float t) { motorDriver.motorA_ccw(); motorDriver.motorB_cw(); wait(t); moveStop();//// } void moveRight(float t) { motorDriver.motorA_cw(); motorDriver.motorB_ccw(); wait(t); moveStop();//// } int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) { int cnt = 0; while (ir_rx.getState() != ReceiverIR::Received) { cnt++; if (timeout < cnt) { return -1; } } return ir_rx.getData(format, buf, bufsiz * 8); } /** * Display a data. * * @param buf Pointer to a buffer. * @param bitlength Bit length of a data. */ void set_TLC1543(void) { int i; for(i = 0; i < 6; i++){ int value = 0; spi_cs = 0; wait_us(2); value = spi.write(i<<12); spi_cs = 1; wait_us(18); value = value >> 6; result[i] = value; } for(int i=0; i<5; i++){ sensor_values[i] = result[i+1]; // pc.printf("sensor_values[i] = %d\r\n", i, sensor_values[i]); } } void calibrate() { int x; int max_sensor_values[5] = {0,}; int min_sensor_values[5] = {1024,}; int q; for(q=0;q<30;q++){ wait(0.5); printf("count cal : %d\r\n",q); set_TLC1543(); for(x=0;x<5;x++){ if(q==0 || max_sensor_values[x] < sensor_values[x]) max_sensor_values[x] = sensor_values[x]; if(q==0 || min_sensor_values[x] > sensor_values[x]) min_sensor_values[x] = sensor_values[x]; } } for(int i=0; i<5; i++) { if(min_sensor_values[i] > calibratedMax[i]) calibratedMax[i] = min_sensor_values[i]; if(max_sensor_values[i] < calibratedMin[i]) calibratedMin[i] = max_sensor_values[i]; } for(int i = 0 ; i < 5; i++){ pc.printf("cali-Max[%d] : %d\r\n",i,calibratedMax[i]); pc.printf("cali-Min[%d] : %d\r\n",i,calibratedMin[i]); } myOled.printf("End Calibrate\r\n",end-start); myOled.display(); } void readCalibrated(int *sensor_values) { int i; set_TLC1543(); for(i=0; i<5; i++) { int denominator; denominator = calibratedMax[i] - calibratedMin[i]; int x=0; if(denominator != 0) x = (sensor_values[i] - calibratedMin[i])*1000/denominator; if(x<0) x=0; else if(x>1000) x=1000; sensor_values[i] = x; } } int readLine(int *sensor_values) { char i, on_line = 0; long avg; // this is for the weighted total, which is long // before division int sum; // this is for the denominator which is <= 64000 static int last_value=0; // assume initially that the line is left. readCalibrated(sensor_values); avg = 0; sum = 0; int _numSensors = 5; for(i=0;i<_numSensors;i++) { int value = sensor_values[i]; pc.printf("IR%d : %d\r\n",i,value); sensor_values[i] = value; // keep track of whether we see the line at all if(value > 300) { on_line = 1; } // only average in values that are above a noise threshold if(value > 50) { avg += (long)(value) * (i * 1000); sum += value; } } if(!on_line) { // If it last read to the left of center, return 0. if(last_value < (_numSensors-1)*1000/2) return 0; // If it last read to the right of center, return the max. else return (_numSensors-1)*1000; } last_value = avg/sum; if(sum > 4500){ last_value = -1; } return last_value; } void rx_cb(void) { char ch; ch = pc.getc(); pc.putc(ch); rx_buffer[index++] = ch; if (ch == 0x0D) { //CR pc.putc(0x0A); //LF rx_buffer[--index] = '\0'; //change CR to 0 index = 0; flag = 1; } } void turnonLED(){ ws.useII(WS28121::PER_PIXEL); // use per-pixel intensity scaling // set up the colours we want to draw with int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f}; // for each of the colours (j) write out 10 of them // the pixels are written at the colour*10, plus the colour position // all modulus 60 so it wraps around for (int i = 0; i < WS2812_BUF; i++) { px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]); } // now all the colours are computed, add a fade effect using intensity scaling // compute and write the II value for each pixel for (int j=0; j<WS2812_BUF; j++) { // px.SetI(pixel position, II value) px.SetI(j%WS2812_BUF, 0xf+(0xf*(j%NUM_LEDS_PER_COLOR))); } // Now the buffer is written, rotate it // by writing it out with an increasing offset while (1) { for (int z=WS2812_BUF; z >= 0 ; z--) { ws.write_offsets(px.getBuf(),z,z,z); wait(0.075); } } } void display_data(uint8_t *buf, int bitlength) { const int n = bitlength / 8 + (((bitlength % 8) != 0) ? 1 : 0); switch(buf[3]){ case 0xF3: // 1, forward moveForward(0.2); wait(0.2); moveStop(); break; case 0xE7: //2, stop moveStop(); break; case 0xA1: // 3, backward moveBackward(0.2); wait(0.2); moveStop(); break; case 0xF7: // 4, right moveRight(0.1); break; case 0xE3: // 5, left moveLeft(0.1); break; case 0xBD: // 7 moveForward(0.05); wait(0.05); moveStop(); break; case 0xAD: // 8 moveBackward(0.05); wait(0.05); moveStop(); break; case 0xB5: // 9 motorDriver.motorA_ccw(); motorDriver.motorB_cw(); break; case 0xBA: // ch- moveForward(0.1); wait(0.1); moveStop(); cali_thread.start(&calibrate); break; case 0xB9: // ch pc.printf("start!\r\n"); char data_write[2]; char data_read[2]; data_write[0] = 0xE0; i2c.write((I2C_ADDR << 1), data_write, 1); i2c.frequency(100000); // 100 k t.start(); start = t.read_ms(); while(1) { pc.printf("--------------------\r\n"); set_TLC1543(); int position = readLine(sensor_values); pc.printf("position : %d\r\n", position); int distance = hcsr04(); pc.printf("distance: %d\r\n",distance); i2c.read(((I2C_ADDR << 1) | 0x01), data_read, 1, 0); int tempval = data_read[0]; /* if(tempval == 0x60){ pc.printf("+++++++++left++++++++++\r\n"); //left moveRight(0.06); wait(1); moveForward(0.21 ); wait(1); moveLeft(0.06); do{ moveForward(0.06); wait(0.2); readCalibrated(sensor_values); wait(0.3); }while(sensor_values[3] <700); continue; } else if(tempval == 0xA0){ pc.printf("===========right========\r\n"); //right moveLeft(0.06); wait(1); moveForward(0.22 ); wait(1); moveRight(0.09); do{ moveForward(0.06); wait(0.2); readCalibrated(sensor_values); wait(0.3); }while(sensor_values[3] <700); continue; } */ /* if(tempval == 0x60){ pc.printf("+++++++++left++++++++++\r\n"); //left moveRight(0.06); wait(1); moveForward(0.24 ); wait(1); moveLeft(0.06); do{ moveForward(0.06); wait(0.2); readCalibrated(sensor_values); wait(0.3); }while(sensor_values[3] <700); continue; } else if(tempval == 0xA0){ pc.printf("===========right========\r\n"); //right moveLeft(0.06); wait(1); moveForward(0.24 ); wait(1); moveRight(0.09); do{ moveForward(0.06); wait(0.2); readCalibrated(sensor_values); wait(0.3); }while(sensor_values[3] <700); continue; } */ if(distance <=21){ pc.printf("stop!!!!please!!\r\n"); moveStop(); //// moveLeft(0.09); } if(position >= 1750 && position < 2600){ moveForward(0.08); } else if(position >= 2600 && position < 3000){ moveRight(0.04); moveStop();//// } else if(position <= 3500 && position >= 3000){ moveRight(0.03); } else if(position <= 4000 && position > 3500){ moveRight(0.02); } else if(position < 1750 && position > 1000){ moveLeft(0.04); } else if(position <= 1000 && position > 0){ moveLeft(0.03); } else if(position == 0){ moveLeft(0.02); } else if(position == -1){ moveStop(); end = t.read_ms(); pc.printf("stop!!!!\r\n"); pc.printf("Total time is %d ms.\r\n", end-start); myOled.printf("Total time is %d ms.\r\n",end-start); myOled.display(); LED_thread.start(&turnonLED); wait(5); } wait(0.1); } break; } } void rx_thread() { while (1) { wait(0.2); uint8_t buf1[32]; int bitlength1; RemoteIR::Format format; memset(buf1, 0x00, sizeof(buf1)); bitlength1 = receive(&format, buf1, sizeof(buf1)); if (bitlength1 < 0) { continue; } display_data(buf1, bitlength1); } } int main() { float fPwmPeriod = 0.00002f;//50KHz float fPwmPulsewidth = 0.250;//duty cycle 0.0~1.0 -> 0%~100% /// motorDriver.setPwmAperiod(fPwmPeriod);//set PWM Period motorDriver.setPwmBperiod(fPwmPeriod); motorDriver.setPwmApulsewidth(fPwmPulsewidth);//set PWM Puls Width motorDriver.setPwmBpulsewidth(fPwmPulsewidth); sock_thread.start(&rx_thread); spi.format(16, 0); spi.frequency(2000000); pc.printf("Hello!\r\n"); pc.attach(callback(rx_cb)); //OLED myOled.begin(); myOled.printf("\nHello World\r\n", myOled.height()); myOled.display(); for(int i = 0; i < 5; i++) calibratedMin[i] = 1023; //fin thread sock_thread.join(); cali_thread.join(); LED_thread.join(); }