190605
Dependencies: WS2812 RemoteIR PixelArray Adafruit_GFX
main.cpp
- Committer:
- Jeonghoon
- Date:
- 2019-06-12
- Revision:
- 107:94134c6f90e8
- Parent:
- 106:d14f340f1fe0
- Child:
- 108:64e7d7025e2f
File content as of revision 107:94134c6f90e8:
#include "mbed.h" //#include "motor.h" #include "ReceiverIR.h" #include "Adafruit_SSD1306.h" // OLED #include "WS2812.h" // botom led #include "PixelArray.h" // bottom led #define NUM_SENSORS 5 #define PCF8574_ADDR 0x20 //0b0010_0000 #define WS2812_BUF 10 #define NUM_COLORS 6 #define NUM_LEDS_PER_COLOR 4 ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4); SPI spi(D11, D12, D13); DigitalOut spi_cs(D10, 1); RawSerial pc(SERIAL_TX, SERIAL_RX, 115200); I2C i2c(I2C_SDA, I2C_SCL); // D14, D15 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); // D9, (D8: D/C - data/command change) DigitalOut DataComm(D8); PixelArray px(WS2812_BUF); WS2812 ws(D7, WS2812_BUF, 6, 17, 9, 14); Thread TR_thread; Thread remote_thread; Thread oled_thread; // Timer Timer timer_ms; Timer timer_s; Timer timer_pid; unsigned long avg = 0; unsigned int sum = 0; unsigned int sensor_values[NUM_SENSORS]; unsigned int max_sensor_values[NUM_SENSORS]; unsigned int min_sensor_values[NUM_SENSORS]; unsigned int *calibratedMin; unsigned int *calibratedMax; int value; int on_line = 0; static int pos = 0; unsigned int last_proportional = 0; long integral = 0; volatile int flag = 0; volatile int start = 0; volatile int timer_flag = 0; int ch; int min_t=0; int sec_t=0; int msec_t=0; int currTime = 0; int prevTime = 0; int dt ; char data_write[1] ; char data_read[1] ; int status; int count_stop; int err; int pid; int readLine(unsigned int *sensor_values); void tr_ready(void); void calibration(void); void init(void); void i2c_init(void); void tracing_pid(void); void set_ir_val(void); void get_pos(void); int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100); void remote_ctrl(void); void oled_disp(void); void bottom_led(void); void control(void); int main() { pc.printf("start\r\n"); spi.format(16, 0); spi.frequency(2000000); i2c_init(); init(); oled_thread.start(&oled_disp); remote_thread.start(&remote_ctrl); while(!motor.cal_start); calibration(); while(!flag); TR_thread.start(&tr_ready); while(!start); pc.printf("motor forward\r\n"); motor.forward(); timer_ms.start(); timer_s.start(); while(!timer_flag); msec_t = timer_ms.read_ms(); msec_t %= 1000; sec_t = timer_s.read(); if(sec_t > 60){ min_t = 1; sec_t %= 60; } while (1); } void bottom_led(void){ int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f}; for (int i = 0; i < WS2812_BUF; i++) { px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]); } for (int j=0; j<WS2812_BUF; j++) { px.SetI(j%WS2812_BUF, 0x4B); } for (int z=WS2812_BUF; z >= 0 ; z--) { ws.write_offsets(px.getBuf(),z,z,z); wait(1); } } void remote_ctrl(void){ while(1){ uint8_t buf1[32]; uint8_t buf2[32]; int bitlength1; RemoteIR::Format format; memset(buf1, 0x00, sizeof(buf1)); memset(buf2, 0x00, sizeof(buf2)); bitlength1 = receive(&format, buf1, sizeof(buf1)); if (bitlength1 < 0) { continue; } } } void init(void){ motor.cal_start = 0; motor.cal_stop = 0; motor.kp = 10.0; motor.kd = 1.0; // motor.ki = 5000; motor.max = 100; DataComm = 0; calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS); calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS); for(int i=0; i < NUM_SENSORS; i++) { calibratedMin[i] = 1023; calibratedMax[i] = 0; } } void i2c_init(void){ i2c.frequency(100000); data_write[0] = 0xE0; //1110_0000 status = i2c.write(PCF8574_ADDR << 1, data_write, 1, 0); if(status != 0){ pc.printf("configuration err\r\n"); while(1){} } } void oled_disp(void){ myOled.begin(); while(1){ myOled.printf("timer: %d:%d:%d sec\r",min_t, sec_t, msec_t); // myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr); myOled.display(); } } int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) { int cnt = 0; while (motor.getState() != ReceiverIR::Received) { cnt++; if (timeout < cnt) { return -1; } } return motor.getData(format, buf, bufsiz * 8); } void read_ir(void){ ch = 0; spi_cs = 0; wait_us(2); value = spi.write(ch << 12); spi_cs = 1; wait_us(21); for(int i = 0; i < 5; i++){ ch += 1; spi_cs = 0; wait_us(2); value = spi.write(ch << 12); spi_cs = 1; wait_us(21); value = value >> 6; value = value * 3300 / 0x3FF; sensor_values[i] = value; } } void calibration(void){ pc.printf("calibration start \r\n"); int i = 0; while(!motor.cal_stop){ read_ir(); for(int j = 0; j < NUM_SENSORS; j++){ if(i == 0 || max_sensor_values[j] < sensor_values[j]){ max_sensor_values[j] = sensor_values[j]; } if(i == 0 || min_sensor_values[j] > sensor_values[j]){ min_sensor_values[j] = sensor_values[j]; } } i = 1; } for(int i = 0; i < NUM_SENSORS; i++){ if(min_sensor_values[i] < calibratedMin[i]) calibratedMin[i] = min_sensor_values[i]; if(max_sensor_values[i] > calibratedMax[i]) calibratedMax[i] = max_sensor_values[i]; } pc.printf("calibration complete\r\n"); flag = 1; } void tr_ready(void){ while(1){ // read current IR 1 ~ IR 5 read_ir(); // set range under 1000 set_ir_val(); // get current position get_pos(); pc.printf("pos: %d\r\n", pos); pc.printf("on_line: %d\r\n\n", on_line); tracing_pid(); } } void set_ir_val(){ for(int i = 0; i < NUM_SENSORS; i++) { unsigned int denominator; denominator = calibratedMax[i] - calibratedMin[i]; signed int x = 0; if(denominator != 0) x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator; if(x < 0) x = 0; else if(x > 1000) x = 1000; sensor_values[i] = x; } // finish line if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){ count_stop ++; if(count_stop > 7){ timer_flag = 1; motor.stop(); bottom_led(); } } else{ count_stop = 0; } } void get_pos(){ on_line = 0; avg = 0; sum = 0; for(int i = 0; i < NUM_SENSORS; i++){ int val = sensor_values[i]; // determine "on_line" or "out_line" if(val < 450){ on_line = 1; } // under if(val > 5){ avg += (long)(val) * (i * 1000); sum += val; } } // out_line if(!on_line){ if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000) pos = 0; // last_vlaue = 0 } else{ // right -> out-line (over 2000) pos = (NUM_SENSORS - 1) * 1000; // pos = 4000 } } // on_line else{ pos = avg / sum; } start = 1; } void tracing_pid(void){ int proportional = pos - 2000; int derivative = proportional - last_proportional; integral += proportional; last_proportional = proportional; int power_difference = proportional / motor.kp+ derivative * motor.kd; const float std = 990.0; if(power_difference > motor.max) power_difference = motor.max; if(power_difference < -motor.max) power_difference = -motor.max; // bot position: right if(power_difference > 15){ //0.1 + p_d/std motor.speed_r((motor.max + power_difference)/std); motor.speed_l((motor.max - power_difference)/std); } // bot position: left else if(power_difference < -15){ motor.speed_r((motor.max + power_difference)/std); motor.speed_l((motor.max - power_difference)/std); } else{ //online motor.speed_l(motor.init_sp_l); motor.speed_r(motor.init_sp_r); } }