190605
Dependencies: WS2812 RemoteIR PixelArray Adafruit_GFX
main.cpp
- Committer:
- Jeonghoon
- Date:
- 2019-06-05
- Revision:
- 100:0e44e944a19f
- Parent:
- 97:bad45e2dc7e1
- Child:
- 101:efa2315d0312
File content as of revision 100:0e44e944a19f:
#include "mbed.h" //#include "motor.h" #include "ReceiverIR.h" #include "Adafruit_SSD1306.h" // OLED #define NUM_SENSORS 5 //Motor motor(D6, D5, PA_0, PA_1, PB_0, PA_4, 0.3); 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); Serial pc(SERIAL_TX, SERIAL_RX, 115200); I2C i2c(D14, D15); // D14, D15 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); // D9, (D8: D/C - data/command change) DigitalOut DataComm(D8); Thread TR_thread; Thread remote_thread; Thread oled_thread; 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; float bat; int on_line = 0; static int pos = 0; unsigned int last_proportional = 0; long integral = 0; volatile int flag = 0; volatile int start = 0; int ch; int readLine(unsigned int *sensor_values); void tr_ready(void); void calibration(void); void 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); int main() { pc.printf("start\r\n"); spi.format(16, 0); spi.frequency(2000000); 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(); while (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; motor.kd = 2; 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 oled_disp(void){ myOled.begin(); while(1){ myOled.clearDisplay(); //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos); myOled.printf("kp:%.2f, kd: %d\r", motor.kp, motor.kd); 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(); 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){ motor.stop(); wait(5); } for(int i = 0; i < NUM_SENSORS; i++){ pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]); } pc.printf("\r\n"); } 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 < 500){ 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 / 20 + integral / 10000 + derivative * 10; //int power_difference = proportional / motor.kp; int power_difference = proportional/motor.kp + derivative * motor.kd; const float std = 600.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){ 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); } // pc.printf("proportional: %d\r\n", proportional); //pc.printf("(max + power_difference): %d\r\n", motor.max + power_difference); // pc.printf("derivative: %d\r\n", derivative); // pc.printf("integral: %d\r\n",integral); }