embedded / Mbed OS line_tracing

190605

Dependencies:   WS2812 RemoteIR PixelArray Adafruit_GFX

main.cpp@112:744fef64b8ba, 2019-06-29 (annotated)

Committer:
ssuda
Date:
Sat Jun 29 14:31:52 2019 +0000
Revision:
112:744fef64b8ba
Parent:
111:dc3d8ba48f83 
19.6.29

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jeonghoon 100:0e44e944a19f 1 #include "mbed.h"
Jeonghoon 100:0e44e944a19f 2 //#include "motor.h"
Jeonghoon 100:0e44e944a19f 3 #include "ReceiverIR.h"
Jeonghoon 100:0e44e944a19f 4 #include "Adafruit_SSD1306.h" // OLED
Jeonghoon 104:94ed54c74466 5 #include "WS2812.h" // botom led
Jeonghoon 104:94ed54c74466 6 #include "PixelArray.h" // bottom led
Jeonghoon 104:94ed54c74466 7
Jeonghoon 104:94ed54c74466 8
Jeonghoon 100:0e44e944a19f 9 #define NUM_SENSORS 5
Jeonghoon 104:94ed54c74466 10 #define PCF8574_ADDR 0x20 //0b0010_0000
Jeonghoon 104:94ed54c74466 11
Jeonghoon 104:94ed54c74466 12 #define WS2812_BUF 10
Jeonghoon 104:94ed54c74466 13 #define NUM_COLORS 6
Jeonghoon 104:94ed54c74466 14 #define NUM_LEDS_PER_COLOR 4
Jeonghoon 106:d14f340f1fe0 15
Jeonghoon 100:0e44e944a19f 16 ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4);
Jeonghoon 100:0e44e944a19f 17 SPI spi(D11, D12, D13);
Jeonghoon 100:0e44e944a19f 18 DigitalOut spi_cs(D10, 1);
Jeonghoon 106:d14f340f1fe0 19 RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
Jeonghoon 104:94ed54c74466 20 I2C i2c(I2C_SDA, I2C_SCL); // D14, D15
Jeonghoon 100:0e44e944a19f 21 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); // D9, (D8: D/C - data/command change)
Jeonghoon 100:0e44e944a19f 22 DigitalOut DataComm(D8);
Jeonghoon 103:b417a6c65a6f 23
Jeonghoon 104:94ed54c74466 24 PixelArray px(WS2812_BUF);
Jeonghoon 104:94ed54c74466 25 WS2812 ws(D7, WS2812_BUF, 6, 17, 9, 14);
Jeonghoon 100:0e44e944a19f 26
Jeonghoon 100:0e44e944a19f 27 Thread TR_thread;
Jeonghoon 100:0e44e944a19f 28 Thread remote_thread;
Jeonghoon 100:0e44e944a19f 29 Thread oled_thread;
Jeonghoon 104:94ed54c74466 30
Jeonghoon 104:94ed54c74466 31
Jeonghoon 104:94ed54c74466 32 // Timer
Jeonghoon 104:94ed54c74466 33 Timer timer_ms;
Jeonghoon 104:94ed54c74466 34 Timer timer_s;
Jeonghoon 104:94ed54c74466 35 Timer timer_pid;
Jeonghoon 100:0e44e944a19f 36
Jeonghoon 100:0e44e944a19f 37 unsigned long avg = 0;
Jeonghoon 100:0e44e944a19f 38 unsigned int sum = 0;
Jeonghoon 100:0e44e944a19f 39
Jeonghoon 100:0e44e944a19f 40 unsigned int sensor_values[NUM_SENSORS];
Jeonghoon 100:0e44e944a19f 41 unsigned int max_sensor_values[NUM_SENSORS];
Jeonghoon 100:0e44e944a19f 42 unsigned int min_sensor_values[NUM_SENSORS];
Jeonghoon 100:0e44e944a19f 43
Jeonghoon 100:0e44e944a19f 44 unsigned int *calibratedMin;
Jeonghoon 100:0e44e944a19f 45 unsigned int *calibratedMax;
Jeonghoon 100:0e44e944a19f 46
Jeonghoon 100:0e44e944a19f 47 int value;
Jeonghoon 100:0e44e944a19f 48 int on_line = 0;
Jeonghoon 100:0e44e944a19f 49 static int pos = 0;
Jeonghoon 100:0e44e944a19f 50 unsigned int last_proportional = 0;
Jeonghoon 100:0e44e944a19f 51 long integral = 0;
Jeonghoon 100:0e44e944a19f 52
Jeonghoon 100:0e44e944a19f 53 volatile int flag = 0;
Jeonghoon 100:0e44e944a19f 54 volatile int start = 0;
Jeonghoon 104:94ed54c74466 55 volatile int timer_flag = 0;
Jeonghoon 104:94ed54c74466 56
Jeonghoon 100:0e44e944a19f 57 int ch;
Jeonghoon 103:b417a6c65a6f 58
Jeonghoon 104:94ed54c74466 59 int min_t=0;
Jeonghoon 104:94ed54c74466 60 int sec_t=0;
Jeonghoon 104:94ed54c74466 61 int msec_t=0;
Jeonghoon 104:94ed54c74466 62
Jeonghoon 104:94ed54c74466 63 int currTime = 0;
Jeonghoon 104:94ed54c74466 64 int prevTime = 0;
Jeonghoon 104:94ed54c74466 65 int dt ;
Jeonghoon 104:94ed54c74466 66
Jeonghoon 104:94ed54c74466 67 char data_write[1] ;
Jeonghoon 104:94ed54c74466 68 char data_read[1] ;
Jeonghoon 104:94ed54c74466 69 int status;
Jeonghoon 104:94ed54c74466 70
Jeonghoon 104:94ed54c74466 71 int count_stop;
Jeonghoon 106:d14f340f1fe0 72 int err;
Jeonghoon 106:d14f340f1fe0 73 int pid;
Jeonghoon 100:0e44e944a19f 74
Jeonghoon 100:0e44e944a19f 75 int readLine(unsigned int *sensor_values);
Jeonghoon 100:0e44e944a19f 76 void tr_ready(void);
Jeonghoon 100:0e44e944a19f 77 void calibration(void);
Jeonghoon 100:0e44e944a19f 78 void init(void);
Jeonghoon 104:94ed54c74466 79 void i2c_init(void);
Jeonghoon 100:0e44e944a19f 80
Jeonghoon 100:0e44e944a19f 81 void tracing_pid(void);
Jeonghoon 100:0e44e944a19f 82 void set_ir_val(void);
Jeonghoon 100:0e44e944a19f 83 void get_pos(void);
Jeonghoon 100:0e44e944a19f 84 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
Jeonghoon 100:0e44e944a19f 85 void remote_ctrl(void);
Jeonghoon 100:0e44e944a19f 86 void oled_disp(void);
Jeonghoon 104:94ed54c74466 87
Jeonghoon 104:94ed54c74466 88 void bottom_led(void);
Jeonghoon 106:d14f340f1fe0 89 void control(void);
Jeonghoon 100:0e44e944a19f 90
Jeonghoon 100:0e44e944a19f 91 int main()
Jeonghoon 100:0e44e944a19f 92 {
Jeonghoon 104:94ed54c74466 93
Jeonghoon 100:0e44e944a19f 94 pc.printf("start\r\n");
Jeonghoon 100:0e44e944a19f 95 spi.format(16, 0);
Jeonghoon 100:0e44e944a19f 96 spi.frequency(2000000);
Jeonghoon 104:94ed54c74466 97
Jeonghoon 104:94ed54c74466 98 i2c_init();
Jeonghoon 100:0e44e944a19f 99 init();
Jeonghoon 100:0e44e944a19f 100
Jeonghoon 111:dc3d8ba48f83 101
Jeonghoon 100:0e44e944a19f 102 remote_thread.start(&remote_ctrl);
Jeonghoon 104:94ed54c74466 103
Jeonghoon 100:0e44e944a19f 104 while(!motor.cal_start);
Jeonghoon 100:0e44e944a19f 105
Jeonghoon 100:0e44e944a19f 106 calibration();
Jeonghoon 100:0e44e944a19f 107
Jeonghoon 100:0e44e944a19f 108 while(!flag);
Jeonghoon 100:0e44e944a19f 109
Jeonghoon 100:0e44e944a19f 110 TR_thread.start(&tr_ready);
Jeonghoon 100:0e44e944a19f 111
Jeonghoon 100:0e44e944a19f 112
Jeonghoon 100:0e44e944a19f 113 while(!start);
Jeonghoon 104:94ed54c74466 114
Jeonghoon 100:0e44e944a19f 115 pc.printf("motor forward\r\n");
Jeonghoon 104:94ed54c74466 116
Jeonghoon 100:0e44e944a19f 117 motor.forward();
Jeonghoon 104:94ed54c74466 118 timer_ms.start();
Jeonghoon 104:94ed54c74466 119 timer_s.start();
Jeonghoon 104:94ed54c74466 120
Jeonghoon 104:94ed54c74466 121 while(!timer_flag);
Jeonghoon 111:dc3d8ba48f83 122 oled_thread.start(&oled_disp);
Jeonghoon 104:94ed54c74466 123 msec_t = timer_ms.read_ms();
Jeonghoon 104:94ed54c74466 124 msec_t %= 1000;
Jeonghoon 104:94ed54c74466 125 sec_t = timer_s.read();
Jeonghoon 104:94ed54c74466 126
Jeonghoon 107:94134c6f90e8 127 if(sec_t > 60){
Jeonghoon 107:94134c6f90e8 128 min_t = 1;
Jeonghoon 107:94134c6f90e8 129 sec_t %= 60;
Jeonghoon 107:94134c6f90e8 130 }
Jeonghoon 107:94134c6f90e8 131
Jeonghoon 100:0e44e944a19f 132 while (1);
Jeonghoon 100:0e44e944a19f 133 }
Jeonghoon 104:94ed54c74466 134
Jeonghoon 104:94ed54c74466 135
Jeonghoon 104:94ed54c74466 136 void bottom_led(void){
Jeonghoon 104:94ed54c74466 137
Jeonghoon 104:94ed54c74466 138 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
Jeonghoon 104:94ed54c74466 139
Jeonghoon 104:94ed54c74466 140 for (int i = 0; i < WS2812_BUF; i++) {
Jeonghoon 104:94ed54c74466 141 px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
Jeonghoon 104:94ed54c74466 142 }
Jeonghoon 104:94ed54c74466 143
Jeonghoon 104:94ed54c74466 144 for (int j=0; j<WS2812_BUF; j++) {
Jeonghoon 104:94ed54c74466 145 px.SetI(j%WS2812_BUF, 0x4B);
Jeonghoon 104:94ed54c74466 146 }
Jeonghoon 104:94ed54c74466 147
Jeonghoon 104:94ed54c74466 148 for (int z=WS2812_BUF; z >= 0 ; z--) {
Jeonghoon 104:94ed54c74466 149 ws.write_offsets(px.getBuf(),z,z,z);
Jeonghoon 104:94ed54c74466 150 wait(1);
Jeonghoon 104:94ed54c74466 151 }
Jeonghoon 104:94ed54c74466 152 }
Jeonghoon 104:94ed54c74466 153
Jeonghoon 100:0e44e944a19f 154 void remote_ctrl(void){
Jeonghoon 100:0e44e944a19f 155
Jeonghoon 100:0e44e944a19f 156 while(1){
Jeonghoon 100:0e44e944a19f 157 uint8_t buf1[32];
Jeonghoon 100:0e44e944a19f 158 uint8_t buf2[32];
Jeonghoon 100:0e44e944a19f 159 int bitlength1;
Jeonghoon 100:0e44e944a19f 160 RemoteIR::Format format;
Jeonghoon 100:0e44e944a19f 161
Jeonghoon 100:0e44e944a19f 162 memset(buf1, 0x00, sizeof(buf1));
Jeonghoon 100:0e44e944a19f 163 memset(buf2, 0x00, sizeof(buf2));
Jeonghoon 100:0e44e944a19f 164
Jeonghoon 100:0e44e944a19f 165 bitlength1 = receive(&format, buf1, sizeof(buf1));
Jeonghoon 100:0e44e944a19f 166 if (bitlength1 < 0) {
Jeonghoon 100:0e44e944a19f 167 continue;
Jeonghoon 100:0e44e944a19f 168 }
Jeonghoon 100:0e44e944a19f 169
Jeonghoon 100:0e44e944a19f 170 }
Jeonghoon 100:0e44e944a19f 171 }
Jeonghoon 100:0e44e944a19f 172
Jeonghoon 100:0e44e944a19f 173 void init(void){
Jeonghoon 100:0e44e944a19f 174 motor.cal_start = 0;
Jeonghoon 100:0e44e944a19f 175 motor.cal_stop = 0;
Jeonghoon 100:0e44e944a19f 176
ssuda 112:744fef64b8ba 177 motor.kp = 12.9;
ssuda 112:744fef64b8ba 178 motor.kd = 2.0;
ssuda 112:744fef64b8ba 179 motor.ki = 6000;
ssuda 112:744fef64b8ba 180 motor.max = 180;
Jeonghoon 100:0e44e944a19f 181
Jeonghoon 100:0e44e944a19f 182 DataComm = 0;
Jeonghoon 100:0e44e944a19f 183
Jeonghoon 100:0e44e944a19f 184 calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
Jeonghoon 100:0e44e944a19f 185 calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
Jeonghoon 100:0e44e944a19f 186
Jeonghoon 100:0e44e944a19f 187 for(int i=0; i < NUM_SENSORS; i++)
Jeonghoon 100:0e44e944a19f 188 {
Jeonghoon 100:0e44e944a19f 189 calibratedMin[i] = 1023;
Jeonghoon 100:0e44e944a19f 190 calibratedMax[i] = 0;
Jeonghoon 100:0e44e944a19f 191 }
Jeonghoon 100:0e44e944a19f 192 }
Jeonghoon 104:94ed54c74466 193
Jeonghoon 104:94ed54c74466 194 void i2c_init(void){
Jeonghoon 104:94ed54c74466 195
Jeonghoon 104:94ed54c74466 196 i2c.frequency(100000);
Jeonghoon 104:94ed54c74466 197 data_write[0] = 0xE0; //1110_0000
Jeonghoon 104:94ed54c74466 198
Jeonghoon 104:94ed54c74466 199 status = i2c.write(PCF8574_ADDR << 1, data_write, 1, 0);
Jeonghoon 104:94ed54c74466 200
Jeonghoon 104:94ed54c74466 201 if(status != 0){
Jeonghoon 104:94ed54c74466 202 pc.printf("configuration err\r\n");
Jeonghoon 104:94ed54c74466 203 while(1){}
Jeonghoon 104:94ed54c74466 204 }
Jeonghoon 104:94ed54c74466 205
Jeonghoon 104:94ed54c74466 206 }
Jeonghoon 100:0e44e944a19f 207
Jeonghoon 100:0e44e944a19f 208 void oled_disp(void){
Jeonghoon 100:0e44e944a19f 209 myOled.begin();
Jeonghoon 100:0e44e944a19f 210 while(1){
Jeonghoon 107:94134c6f90e8 211 myOled.printf("timer: %d:%d:%d sec\r",min_t, sec_t, msec_t);
Jeonghoon 106:d14f340f1fe0 212 // myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr);
Jeonghoon 100:0e44e944a19f 213 myOled.display();
Jeonghoon 107:94134c6f90e8 214
Jeonghoon 100:0e44e944a19f 215 }
Jeonghoon 100:0e44e944a19f 216 }
Jeonghoon 100:0e44e944a19f 217
Jeonghoon 100:0e44e944a19f 218
Jeonghoon 100:0e44e944a19f 219 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
Jeonghoon 100:0e44e944a19f 220 int cnt = 0;
Jeonghoon 100:0e44e944a19f 221 while (motor.getState() != ReceiverIR::Received) {
Jeonghoon 100:0e44e944a19f 222 cnt++;
Jeonghoon 100:0e44e944a19f 223 if (timeout < cnt) {
Jeonghoon 100:0e44e944a19f 224 return -1;
Jeonghoon 100:0e44e944a19f 225 }
Jeonghoon 100:0e44e944a19f 226 }
Jeonghoon 100:0e44e944a19f 227 return motor.getData(format, buf, bufsiz * 8);
Jeonghoon 100:0e44e944a19f 228 }
Jeonghoon 100:0e44e944a19f 229
Jeonghoon 100:0e44e944a19f 230
Jeonghoon 100:0e44e944a19f 231 void read_ir(void){
Jeonghoon 100:0e44e944a19f 232 ch = 0;
Jeonghoon 100:0e44e944a19f 233
Jeonghoon 100:0e44e944a19f 234 spi_cs = 0;
Jeonghoon 100:0e44e944a19f 235 wait_us(2);
Jeonghoon 100:0e44e944a19f 236 value = spi.write(ch << 12);
Jeonghoon 100:0e44e944a19f 237 spi_cs = 1;
Jeonghoon 100:0e44e944a19f 238
Jeonghoon 100:0e44e944a19f 239 wait_us(21);
Jeonghoon 100:0e44e944a19f 240
Jeonghoon 100:0e44e944a19f 241 for(int i = 0; i < 5; i++){
Jeonghoon 100:0e44e944a19f 242
Jeonghoon 100:0e44e944a19f 243 ch += 1;
Jeonghoon 100:0e44e944a19f 244
Jeonghoon 100:0e44e944a19f 245 spi_cs = 0;
Jeonghoon 100:0e44e944a19f 246 wait_us(2);
Jeonghoon 100:0e44e944a19f 247 value = spi.write(ch << 12);
Jeonghoon 100:0e44e944a19f 248 spi_cs = 1;
Jeonghoon 100:0e44e944a19f 249
Jeonghoon 100:0e44e944a19f 250 wait_us(21);
Jeonghoon 100:0e44e944a19f 251
Jeonghoon 100:0e44e944a19f 252 value = value >> 6;
Jeonghoon 100:0e44e944a19f 253 value = value * 3300 / 0x3FF;
Jeonghoon 100:0e44e944a19f 254
Jeonghoon 100:0e44e944a19f 255 sensor_values[i] = value;
Jeonghoon 100:0e44e944a19f 256 }
Jeonghoon 107:94134c6f90e8 257
Jeonghoon 106:d14f340f1fe0 258
Jeonghoon 100:0e44e944a19f 259 }
Jeonghoon 100:0e44e944a19f 260
Jeonghoon 100:0e44e944a19f 261 void calibration(void){
Jeonghoon 100:0e44e944a19f 262 pc.printf("calibration start \r\n");
Jeonghoon 100:0e44e944a19f 263
Jeonghoon 100:0e44e944a19f 264 int i = 0;
Jeonghoon 100:0e44e944a19f 265 while(!motor.cal_stop){
Jeonghoon 100:0e44e944a19f 266 read_ir();
Jeonghoon 100:0e44e944a19f 267 for(int j = 0; j < NUM_SENSORS; j++){
Jeonghoon 100:0e44e944a19f 268 if(i == 0 || max_sensor_values[j] < sensor_values[j]){
Jeonghoon 100:0e44e944a19f 269 max_sensor_values[j] = sensor_values[j];
Jeonghoon 100:0e44e944a19f 270 }
Jeonghoon 100:0e44e944a19f 271
Jeonghoon 100:0e44e944a19f 272 if(i == 0 || min_sensor_values[j] > sensor_values[j]){
Jeonghoon 100:0e44e944a19f 273 min_sensor_values[j] = sensor_values[j];
Jeonghoon 100:0e44e944a19f 274 }
Jeonghoon 100:0e44e944a19f 275 }
Jeonghoon 100:0e44e944a19f 276 i = 1;
Jeonghoon 100:0e44e944a19f 277 }
Jeonghoon 100:0e44e944a19f 278
Jeonghoon 100:0e44e944a19f 279 for(int i = 0; i < NUM_SENSORS; i++){
Jeonghoon 100:0e44e944a19f 280 if(min_sensor_values[i] < calibratedMin[i])
Jeonghoon 100:0e44e944a19f 281 calibratedMin[i] = min_sensor_values[i];
Jeonghoon 100:0e44e944a19f 282 if(max_sensor_values[i] > calibratedMax[i])
Jeonghoon 100:0e44e944a19f 283 calibratedMax[i] = max_sensor_values[i];
Jeonghoon 100:0e44e944a19f 284 }
Jeonghoon 100:0e44e944a19f 285
Jeonghoon 100:0e44e944a19f 286 pc.printf("calibration complete\r\n");
Jeonghoon 100:0e44e944a19f 287 flag = 1;
Jeonghoon 100:0e44e944a19f 288 }
Jeonghoon 100:0e44e944a19f 289
Jeonghoon 100:0e44e944a19f 290 void tr_ready(void){
ssuda 112:744fef64b8ba 291 unsigned int right_max = 0.8;
ssuda 112:744fef64b8ba 292 unsigned int left_max = 0.805;
ssuda 112:744fef64b8ba 293 // unsigned int sub = 0.5;
Jeonghoon 100:0e44e944a19f 294 while(1){
Jeonghoon 100:0e44e944a19f 295 // read current IR 1 ~ IR 5
Jeonghoon 100:0e44e944a19f 296 read_ir();
Jeonghoon 106:d14f340f1fe0 297
Jeonghoon 100:0e44e944a19f 298 // set range under 1000
Jeonghoon 100:0e44e944a19f 299 set_ir_val();
Jeonghoon 110:13f123179c26 300
Jeonghoon 100:0e44e944a19f 301 // get current position
Jeonghoon 100:0e44e944a19f 302 get_pos();
ssuda 112:744fef64b8ba 303 if(pos<1800){ //turn r
ssuda 112:744fef64b8ba 304 motor.speed_r(right_max);
ssuda 112:744fef64b8ba 305 motor.speed_l(left_max + 0.13);
ssuda 112:744fef64b8ba 306 }
ssuda 112:744fef64b8ba 307 else if(pos>2250){ //turn l
ssuda 112:744fef64b8ba 308 motor.speed_r(right_max + 0.18);
ssuda 112:744fef64b8ba 309 motor.speed_l(left_max - 0.1);
ssuda 112:744fef64b8ba 310 }
ssuda 112:744fef64b8ba 311 else{
ssuda 112:744fef64b8ba 312 tracing_pid();
ssuda 112:744fef64b8ba 313 }
Jeonghoon 100:0e44e944a19f 314 }
Jeonghoon 100:0e44e944a19f 315 }
Jeonghoon 100:0e44e944a19f 316
Jeonghoon 100:0e44e944a19f 317 void set_ir_val(){
Jeonghoon 100:0e44e944a19f 318 for(int i = 0; i < NUM_SENSORS; i++)
Jeonghoon 100:0e44e944a19f 319 {
Jeonghoon 100:0e44e944a19f 320 unsigned int denominator;
Jeonghoon 100:0e44e944a19f 321
Jeonghoon 100:0e44e944a19f 322 denominator = calibratedMax[i] - calibratedMin[i];
Jeonghoon 100:0e44e944a19f 323
Jeonghoon 100:0e44e944a19f 324 signed int x = 0;
Jeonghoon 100:0e44e944a19f 325 if(denominator != 0)
Jeonghoon 100:0e44e944a19f 326 x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
Jeonghoon 100:0e44e944a19f 327 if(x < 0) x = 0;
Jeonghoon 100:0e44e944a19f 328 else if(x > 1000) x = 1000;
Jeonghoon 106:d14f340f1fe0 329
Jeonghoon 100:0e44e944a19f 330 sensor_values[i] = x;
Jeonghoon 106:d14f340f1fe0 331
Jeonghoon 100:0e44e944a19f 332 }
Jeonghoon 110:13f123179c26 333
Jeonghoon 110:13f123179c26 334 for(int i = 0; i < NUM_SENSORS; i++){
Jeonghoon 110:13f123179c26 335 pc.printf("sensor_values[%d]: %d\r\n",i, sensor_values[i]);
Jeonghoon 110:13f123179c26 336 }
Jeonghoon 110:13f123179c26 337 pc.printf("\r\n");
Jeonghoon 106:d14f340f1fe0 338
Jeonghoon 100:0e44e944a19f 339 // finish line
Jeonghoon 110:13f123179c26 340 if(sensor_values[0] < 300 && sensor_values[1] < 300 && sensor_values[2] < 300 && sensor_values[3] < 300 && sensor_values[4] < 300){
Jeonghoon 110:13f123179c26 341 // count_stop ++;
Jeonghoon 110:13f123179c26 342 // if(count_stop > 7){
Jeonghoon 107:94134c6f90e8 343 timer_flag = 1;
Jeonghoon 107:94134c6f90e8 344 motor.stop();
Jeonghoon 107:94134c6f90e8 345
Jeonghoon 107:94134c6f90e8 346 bottom_led();
Jeonghoon 110:13f123179c26 347 // }
Jeonghoon 107:94134c6f90e8 348 }
Jeonghoon 110:13f123179c26 349 // else{
Jeonghoon 110:13f123179c26 350 // count_stop = 0;
Jeonghoon 110:13f123179c26 351 // }
Jeonghoon 104:94ed54c74466 352
Jeonghoon 100:0e44e944a19f 353 }
Jeonghoon 100:0e44e944a19f 354
Jeonghoon 100:0e44e944a19f 355 void get_pos(){
Jeonghoon 100:0e44e944a19f 356 on_line = 0;
Jeonghoon 100:0e44e944a19f 357 avg = 0;
Jeonghoon 100:0e44e944a19f 358 sum = 0;
Jeonghoon 100:0e44e944a19f 359
Jeonghoon 100:0e44e944a19f 360 for(int i = 0; i < NUM_SENSORS; i++){
Jeonghoon 100:0e44e944a19f 361 int val = sensor_values[i];
Jeonghoon 100:0e44e944a19f 362
Jeonghoon 100:0e44e944a19f 363 // determine "on_line" or "out_line"
Jeonghoon 107:94134c6f90e8 364 if(val < 450){
Jeonghoon 100:0e44e944a19f 365 on_line = 1;
Jeonghoon 100:0e44e944a19f 366 }
Jeonghoon 100:0e44e944a19f 367
Jeonghoon 100:0e44e944a19f 368 // under
Jeonghoon 100:0e44e944a19f 369 if(val > 5){
Jeonghoon 100:0e44e944a19f 370 avg += (long)(val) * (i * 1000);
Jeonghoon 100:0e44e944a19f 371 sum += val;
Jeonghoon 100:0e44e944a19f 372 }
Jeonghoon 100:0e44e944a19f 373 }
Jeonghoon 100:0e44e944a19f 374
Jeonghoon 100:0e44e944a19f 375 // out_line
Jeonghoon 100:0e44e944a19f 376 if(!on_line){
Jeonghoon 107:94134c6f90e8 377 if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
Jeonghoon 107:94134c6f90e8 378 pos = 0; // last_vlaue = 0
Jeonghoon 111:dc3d8ba48f83 379 // pos = 200;
Jeonghoon 100:0e44e944a19f 380 }
Jeonghoon 100:0e44e944a19f 381 else{ // right -> out-line (over 2000)
Jeonghoon 107:94134c6f90e8 382 pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
Jeonghoon 111:dc3d8ba48f83 383 // pos = 3800;
Jeonghoon 100:0e44e944a19f 384 }
Jeonghoon 100:0e44e944a19f 385 }
Jeonghoon 100:0e44e944a19f 386 // on_line
Jeonghoon 100:0e44e944a19f 387 else{
Jeonghoon 100:0e44e944a19f 388 pos = avg / sum;
Jeonghoon 100:0e44e944a19f 389 }
Jeonghoon 100:0e44e944a19f 390
Jeonghoon 100:0e44e944a19f 391 start = 1;
Jeonghoon 100:0e44e944a19f 392 }
Jeonghoon 100:0e44e944a19f 393
Jeonghoon 100:0e44e944a19f 394 void tracing_pid(void){
Jeonghoon 100:0e44e944a19f 395
Jeonghoon 107:94134c6f90e8 396 int proportional = pos - 2000;
Jeonghoon 100:0e44e944a19f 397
Jeonghoon 107:94134c6f90e8 398 int derivative = proportional - last_proportional;
Jeonghoon 100:0e44e944a19f 399 integral += proportional;
Jeonghoon 107:94134c6f90e8 400
Jeonghoon 100:0e44e944a19f 401 last_proportional = proportional;
Jeonghoon 100:0e44e944a19f 402
ssuda 112:744fef64b8ba 403 int power_difference = proportional / motor.kp+ integral/motor.ki + derivative * motor.kd;
Jeonghoon 107:94134c6f90e8 404
ssuda 112:744fef64b8ba 405 const float std = 500.0;
Jeonghoon 107:94134c6f90e8 406
Jeonghoon 103:b417a6c65a6f 407 if(power_difference > motor.max)
Jeonghoon 103:b417a6c65a6f 408 power_difference = motor.max;
Jeonghoon 100:0e44e944a19f 409
Jeonghoon 103:b417a6c65a6f 410 if(power_difference < -motor.max)
Jeonghoon 103:b417a6c65a6f 411 power_difference = -motor.max;
Jeonghoon 100:0e44e944a19f 412
Jeonghoon 107:94134c6f90e8 413 // bot position: right
ssuda 112:744fef64b8ba 414 if(power_difference > 10){ //0.1 + p_d/std
Jeonghoon 107:94134c6f90e8 415 motor.speed_r((motor.max + power_difference)/std);
Jeonghoon 111:dc3d8ba48f83 416 motor.speed_l((motor.max - power_difference)/std + 0.005);
Jeonghoon 107:94134c6f90e8 417 }
Jeonghoon 107:94134c6f90e8 418 // bot position: left
ssuda 112:744fef64b8ba 419 else if(power_difference < -10){
Jeonghoon 107:94134c6f90e8 420 motor.speed_r((motor.max + power_difference)/std);
Jeonghoon 111:dc3d8ba48f83 421 motor.speed_l((motor.max - power_difference)/std + 0.005);
Jeonghoon 107:94134c6f90e8 422 }
Jeonghoon 107:94134c6f90e8 423 else{ //online
Jeonghoon 107:94134c6f90e8 424 motor.speed_l(motor.init_sp_l);
Jeonghoon 107:94134c6f90e8 425 motor.speed_r(motor.init_sp_r);
Jeonghoon 107:94134c6f90e8 426 }
Jeonghoon 102:e846a127af54 427
Jeonghoon 106:d14f340f1fe0 428 }
Jeonghoon 106:d14f340f1fe0 429