embedded / Mbed OS line_tracing

190605

Dependencies:   WS2812 RemoteIR PixelArray Adafruit_GFX

main.cpp@110:13f123179c26, 2019-06-12 (annotated)

Committer:
Jeonghoon
Date:
Wed Jun 12 08:22:54 2019 +0000
Revision:
110:13f123179c26
Parent:
109:66ba5700bf74
Child:
111:dc3d8ba48f83
last_stable_test

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 100:0e44e944a19f 101 oled_thread.start(&oled_disp);
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 104:94ed54c74466 122 msec_t = timer_ms.read_ms();
Jeonghoon 104:94ed54c74466 123 msec_t %= 1000;
Jeonghoon 104:94ed54c74466 124 sec_t = timer_s.read();
Jeonghoon 104:94ed54c74466 125
Jeonghoon 107:94134c6f90e8 126 if(sec_t > 60){
Jeonghoon 107:94134c6f90e8 127 min_t = 1;
Jeonghoon 107:94134c6f90e8 128 sec_t %= 60;
Jeonghoon 107:94134c6f90e8 129 }
Jeonghoon 107:94134c6f90e8 130
Jeonghoon 100:0e44e944a19f 131 while (1);
Jeonghoon 100:0e44e944a19f 132 }
Jeonghoon 104:94ed54c74466 133
Jeonghoon 104:94ed54c74466 134
Jeonghoon 104:94ed54c74466 135 void bottom_led(void){
Jeonghoon 104:94ed54c74466 136
Jeonghoon 104:94ed54c74466 137 int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
Jeonghoon 104:94ed54c74466 138
Jeonghoon 104:94ed54c74466 139 for (int i = 0; i < WS2812_BUF; i++) {
Jeonghoon 104:94ed54c74466 140 px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
Jeonghoon 104:94ed54c74466 141 }
Jeonghoon 104:94ed54c74466 142
Jeonghoon 104:94ed54c74466 143 for (int j=0; j<WS2812_BUF; j++) {
Jeonghoon 104:94ed54c74466 144 px.SetI(j%WS2812_BUF, 0x4B);
Jeonghoon 104:94ed54c74466 145 }
Jeonghoon 104:94ed54c74466 146
Jeonghoon 104:94ed54c74466 147 for (int z=WS2812_BUF; z >= 0 ; z--) {
Jeonghoon 104:94ed54c74466 148 ws.write_offsets(px.getBuf(),z,z,z);
Jeonghoon 104:94ed54c74466 149 wait(1);
Jeonghoon 104:94ed54c74466 150 }
Jeonghoon 104:94ed54c74466 151 }
Jeonghoon 104:94ed54c74466 152
Jeonghoon 100:0e44e944a19f 153 void remote_ctrl(void){
Jeonghoon 100:0e44e944a19f 154
Jeonghoon 100:0e44e944a19f 155 while(1){
Jeonghoon 100:0e44e944a19f 156 uint8_t buf1[32];
Jeonghoon 100:0e44e944a19f 157 uint8_t buf2[32];
Jeonghoon 100:0e44e944a19f 158 int bitlength1;
Jeonghoon 100:0e44e944a19f 159 RemoteIR::Format format;
Jeonghoon 100:0e44e944a19f 160
Jeonghoon 100:0e44e944a19f 161 memset(buf1, 0x00, sizeof(buf1));
Jeonghoon 100:0e44e944a19f 162 memset(buf2, 0x00, sizeof(buf2));
Jeonghoon 100:0e44e944a19f 163
Jeonghoon 100:0e44e944a19f 164 bitlength1 = receive(&format, buf1, sizeof(buf1));
Jeonghoon 100:0e44e944a19f 165 if (bitlength1 < 0) {
Jeonghoon 100:0e44e944a19f 166 continue;
Jeonghoon 100:0e44e944a19f 167 }
Jeonghoon 100:0e44e944a19f 168
Jeonghoon 100:0e44e944a19f 169 }
Jeonghoon 100:0e44e944a19f 170 }
Jeonghoon 100:0e44e944a19f 171
Jeonghoon 100:0e44e944a19f 172 void init(void){
Jeonghoon 100:0e44e944a19f 173 motor.cal_start = 0;
Jeonghoon 100:0e44e944a19f 174 motor.cal_stop = 0;
Jeonghoon 100:0e44e944a19f 175
Jeonghoon 107:94134c6f90e8 176 motor.kp = 10.0;
Jeonghoon 108:64e7d7025e2f 177 motor.kd = 0.0;
Jeonghoon 107:94134c6f90e8 178 // motor.ki = 5000;
Jeonghoon 107:94134c6f90e8 179 motor.max = 100;
Jeonghoon 100:0e44e944a19f 180
Jeonghoon 100:0e44e944a19f 181 DataComm = 0;
Jeonghoon 100:0e44e944a19f 182
Jeonghoon 100:0e44e944a19f 183 calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
Jeonghoon 100:0e44e944a19f 184 calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
Jeonghoon 100:0e44e944a19f 185
Jeonghoon 100:0e44e944a19f 186 for(int i=0; i < NUM_SENSORS; i++)
Jeonghoon 100:0e44e944a19f 187 {
Jeonghoon 100:0e44e944a19f 188 calibratedMin[i] = 1023;
Jeonghoon 100:0e44e944a19f 189 calibratedMax[i] = 0;
Jeonghoon 100:0e44e944a19f 190 }
Jeonghoon 100:0e44e944a19f 191 }
Jeonghoon 104:94ed54c74466 192
Jeonghoon 104:94ed54c74466 193 void i2c_init(void){
Jeonghoon 104:94ed54c74466 194
Jeonghoon 104:94ed54c74466 195 i2c.frequency(100000);
Jeonghoon 104:94ed54c74466 196 data_write[0] = 0xE0; //1110_0000
Jeonghoon 104:94ed54c74466 197
Jeonghoon 104:94ed54c74466 198 status = i2c.write(PCF8574_ADDR << 1, data_write, 1, 0);
Jeonghoon 104:94ed54c74466 199
Jeonghoon 104:94ed54c74466 200 if(status != 0){
Jeonghoon 104:94ed54c74466 201 pc.printf("configuration err\r\n");
Jeonghoon 104:94ed54c74466 202 while(1){}
Jeonghoon 104:94ed54c74466 203 }
Jeonghoon 104:94ed54c74466 204
Jeonghoon 104:94ed54c74466 205 }
Jeonghoon 100:0e44e944a19f 206
Jeonghoon 100:0e44e944a19f 207 void oled_disp(void){
Jeonghoon 100:0e44e944a19f 208 myOled.begin();
Jeonghoon 100:0e44e944a19f 209 while(1){
Jeonghoon 107:94134c6f90e8 210 myOled.printf("timer: %d:%d:%d sec\r",min_t, sec_t, msec_t);
Jeonghoon 106:d14f340f1fe0 211 // myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr);
Jeonghoon 100:0e44e944a19f 212 myOled.display();
Jeonghoon 107:94134c6f90e8 213
Jeonghoon 100:0e44e944a19f 214 }
Jeonghoon 100:0e44e944a19f 215 }
Jeonghoon 100:0e44e944a19f 216
Jeonghoon 100:0e44e944a19f 217
Jeonghoon 100:0e44e944a19f 218 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
Jeonghoon 100:0e44e944a19f 219 int cnt = 0;
Jeonghoon 100:0e44e944a19f 220 while (motor.getState() != ReceiverIR::Received) {
Jeonghoon 100:0e44e944a19f 221 cnt++;
Jeonghoon 100:0e44e944a19f 222 if (timeout < cnt) {
Jeonghoon 100:0e44e944a19f 223 return -1;
Jeonghoon 100:0e44e944a19f 224 }
Jeonghoon 100:0e44e944a19f 225 }
Jeonghoon 100:0e44e944a19f 226 return motor.getData(format, buf, bufsiz * 8);
Jeonghoon 100:0e44e944a19f 227 }
Jeonghoon 100:0e44e944a19f 228
Jeonghoon 100:0e44e944a19f 229
Jeonghoon 100:0e44e944a19f 230 void read_ir(void){
Jeonghoon 100:0e44e944a19f 231 ch = 0;
Jeonghoon 100:0e44e944a19f 232
Jeonghoon 100:0e44e944a19f 233 spi_cs = 0;
Jeonghoon 100:0e44e944a19f 234 wait_us(2);
Jeonghoon 100:0e44e944a19f 235 value = spi.write(ch << 12);
Jeonghoon 100:0e44e944a19f 236 spi_cs = 1;
Jeonghoon 100:0e44e944a19f 237
Jeonghoon 100:0e44e944a19f 238 wait_us(21);
Jeonghoon 100:0e44e944a19f 239
Jeonghoon 100:0e44e944a19f 240 for(int i = 0; i < 5; i++){
Jeonghoon 100:0e44e944a19f 241
Jeonghoon 100:0e44e944a19f 242 ch += 1;
Jeonghoon 100:0e44e944a19f 243
Jeonghoon 100:0e44e944a19f 244 spi_cs = 0;
Jeonghoon 100:0e44e944a19f 245 wait_us(2);
Jeonghoon 100:0e44e944a19f 246 value = spi.write(ch << 12);
Jeonghoon 100:0e44e944a19f 247 spi_cs = 1;
Jeonghoon 100:0e44e944a19f 248
Jeonghoon 100:0e44e944a19f 249 wait_us(21);
Jeonghoon 100:0e44e944a19f 250
Jeonghoon 100:0e44e944a19f 251 value = value >> 6;
Jeonghoon 100:0e44e944a19f 252 value = value * 3300 / 0x3FF;
Jeonghoon 100:0e44e944a19f 253
Jeonghoon 100:0e44e944a19f 254 sensor_values[i] = value;
Jeonghoon 100:0e44e944a19f 255 }
Jeonghoon 107:94134c6f90e8 256
Jeonghoon 106:d14f340f1fe0 257
Jeonghoon 100:0e44e944a19f 258 }
Jeonghoon 100:0e44e944a19f 259
Jeonghoon 100:0e44e944a19f 260 void calibration(void){
Jeonghoon 100:0e44e944a19f 261 pc.printf("calibration start \r\n");
Jeonghoon 100:0e44e944a19f 262
Jeonghoon 100:0e44e944a19f 263 int i = 0;
Jeonghoon 100:0e44e944a19f 264 while(!motor.cal_stop){
Jeonghoon 100:0e44e944a19f 265 read_ir();
Jeonghoon 100:0e44e944a19f 266 for(int j = 0; j < NUM_SENSORS; j++){
Jeonghoon 100:0e44e944a19f 267 if(i == 0 || max_sensor_values[j] < sensor_values[j]){
Jeonghoon 100:0e44e944a19f 268 max_sensor_values[j] = sensor_values[j];
Jeonghoon 100:0e44e944a19f 269 }
Jeonghoon 100:0e44e944a19f 270
Jeonghoon 100:0e44e944a19f 271 if(i == 0 || min_sensor_values[j] > sensor_values[j]){
Jeonghoon 100:0e44e944a19f 272 min_sensor_values[j] = sensor_values[j];
Jeonghoon 100:0e44e944a19f 273 }
Jeonghoon 100:0e44e944a19f 274 }
Jeonghoon 100:0e44e944a19f 275 i = 1;
Jeonghoon 100:0e44e944a19f 276 }
Jeonghoon 100:0e44e944a19f 277
Jeonghoon 100:0e44e944a19f 278 for(int i = 0; i < NUM_SENSORS; i++){
Jeonghoon 100:0e44e944a19f 279 if(min_sensor_values[i] < calibratedMin[i])
Jeonghoon 100:0e44e944a19f 280 calibratedMin[i] = min_sensor_values[i];
Jeonghoon 100:0e44e944a19f 281 if(max_sensor_values[i] > calibratedMax[i])
Jeonghoon 100:0e44e944a19f 282 calibratedMax[i] = max_sensor_values[i];
Jeonghoon 100:0e44e944a19f 283 }
Jeonghoon 100:0e44e944a19f 284
Jeonghoon 100:0e44e944a19f 285 pc.printf("calibration complete\r\n");
Jeonghoon 100:0e44e944a19f 286 flag = 1;
Jeonghoon 100:0e44e944a19f 287 }
Jeonghoon 100:0e44e944a19f 288
Jeonghoon 100:0e44e944a19f 289 void tr_ready(void){
Jeonghoon 100:0e44e944a19f 290
Jeonghoon 100:0e44e944a19f 291 while(1){
Jeonghoon 100:0e44e944a19f 292 // read current IR 1 ~ IR 5
Jeonghoon 100:0e44e944a19f 293 read_ir();
Jeonghoon 106:d14f340f1fe0 294
Jeonghoon 100:0e44e944a19f 295 // set range under 1000
Jeonghoon 100:0e44e944a19f 296 set_ir_val();
Jeonghoon 110:13f123179c26 297
Jeonghoon 100:0e44e944a19f 298 // get current position
Jeonghoon 100:0e44e944a19f 299 get_pos();
Jeonghoon 110:13f123179c26 300 // pc.printf("pos: %d\r\n", pos);
Jeonghoon 110:13f123179c26 301 // pc.printf("on_line: %d\r\n\n", on_line);
Jeonghoon 107:94134c6f90e8 302
Jeonghoon 106:d14f340f1fe0 303 tracing_pid();
Jeonghoon 107:94134c6f90e8 304
Jeonghoon 100:0e44e944a19f 305 }
Jeonghoon 100:0e44e944a19f 306 }
Jeonghoon 100:0e44e944a19f 307
Jeonghoon 100:0e44e944a19f 308 void set_ir_val(){
Jeonghoon 100:0e44e944a19f 309 for(int i = 0; i < NUM_SENSORS; i++)
Jeonghoon 100:0e44e944a19f 310 {
Jeonghoon 100:0e44e944a19f 311 unsigned int denominator;
Jeonghoon 100:0e44e944a19f 312
Jeonghoon 100:0e44e944a19f 313 denominator = calibratedMax[i] - calibratedMin[i];
Jeonghoon 100:0e44e944a19f 314
Jeonghoon 100:0e44e944a19f 315 signed int x = 0;
Jeonghoon 100:0e44e944a19f 316 if(denominator != 0)
Jeonghoon 100:0e44e944a19f 317 x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
Jeonghoon 100:0e44e944a19f 318 if(x < 0) x = 0;
Jeonghoon 100:0e44e944a19f 319 else if(x > 1000) x = 1000;
Jeonghoon 106:d14f340f1fe0 320
Jeonghoon 100:0e44e944a19f 321 sensor_values[i] = x;
Jeonghoon 106:d14f340f1fe0 322
Jeonghoon 100:0e44e944a19f 323 }
Jeonghoon 110:13f123179c26 324
Jeonghoon 110:13f123179c26 325 for(int i = 0; i < NUM_SENSORS; i++){
Jeonghoon 110:13f123179c26 326 pc.printf("sensor_values[%d]: %d\r\n",i, sensor_values[i]);
Jeonghoon 110:13f123179c26 327 }
Jeonghoon 110:13f123179c26 328 pc.printf("\r\n");
Jeonghoon 106:d14f340f1fe0 329
Jeonghoon 100:0e44e944a19f 330 // finish line
Jeonghoon 110:13f123179c26 331 if(sensor_values[0] < 300 && sensor_values[1] < 300 && sensor_values[2] < 300 && sensor_values[3] < 300 && sensor_values[4] < 300){
Jeonghoon 110:13f123179c26 332 // count_stop ++;
Jeonghoon 110:13f123179c26 333 // if(count_stop > 7){
Jeonghoon 107:94134c6f90e8 334 timer_flag = 1;
Jeonghoon 107:94134c6f90e8 335 motor.stop();
Jeonghoon 107:94134c6f90e8 336
Jeonghoon 107:94134c6f90e8 337 bottom_led();
Jeonghoon 110:13f123179c26 338 // }
Jeonghoon 107:94134c6f90e8 339 }
Jeonghoon 110:13f123179c26 340 // else{
Jeonghoon 110:13f123179c26 341 // count_stop = 0;
Jeonghoon 110:13f123179c26 342 // }
Jeonghoon 104:94ed54c74466 343
Jeonghoon 100:0e44e944a19f 344 }
Jeonghoon 100:0e44e944a19f 345
Jeonghoon 100:0e44e944a19f 346 void get_pos(){
Jeonghoon 100:0e44e944a19f 347 on_line = 0;
Jeonghoon 100:0e44e944a19f 348 avg = 0;
Jeonghoon 100:0e44e944a19f 349 sum = 0;
Jeonghoon 100:0e44e944a19f 350
Jeonghoon 100:0e44e944a19f 351 for(int i = 0; i < NUM_SENSORS; i++){
Jeonghoon 100:0e44e944a19f 352 int val = sensor_values[i];
Jeonghoon 100:0e44e944a19f 353
Jeonghoon 100:0e44e944a19f 354 // determine "on_line" or "out_line"
Jeonghoon 107:94134c6f90e8 355 if(val < 450){
Jeonghoon 100:0e44e944a19f 356 on_line = 1;
Jeonghoon 100:0e44e944a19f 357 }
Jeonghoon 100:0e44e944a19f 358
Jeonghoon 100:0e44e944a19f 359 // under
Jeonghoon 100:0e44e944a19f 360 if(val > 5){
Jeonghoon 100:0e44e944a19f 361 avg += (long)(val) * (i * 1000);
Jeonghoon 100:0e44e944a19f 362 sum += val;
Jeonghoon 100:0e44e944a19f 363 }
Jeonghoon 100:0e44e944a19f 364 }
Jeonghoon 100:0e44e944a19f 365
Jeonghoon 100:0e44e944a19f 366 // out_line
Jeonghoon 100:0e44e944a19f 367 if(!on_line){
Jeonghoon 107:94134c6f90e8 368 if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
Jeonghoon 107:94134c6f90e8 369 pos = 0; // last_vlaue = 0
Jeonghoon 100:0e44e944a19f 370 }
Jeonghoon 100:0e44e944a19f 371 else{ // right -> out-line (over 2000)
Jeonghoon 107:94134c6f90e8 372 pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
Jeonghoon 100:0e44e944a19f 373 }
Jeonghoon 100:0e44e944a19f 374 }
Jeonghoon 100:0e44e944a19f 375 // on_line
Jeonghoon 100:0e44e944a19f 376 else{
Jeonghoon 100:0e44e944a19f 377 pos = avg / sum;
Jeonghoon 100:0e44e944a19f 378 }
Jeonghoon 100:0e44e944a19f 379
Jeonghoon 100:0e44e944a19f 380 start = 1;
Jeonghoon 100:0e44e944a19f 381 }
Jeonghoon 100:0e44e944a19f 382
Jeonghoon 100:0e44e944a19f 383 void tracing_pid(void){
Jeonghoon 100:0e44e944a19f 384
Jeonghoon 107:94134c6f90e8 385 int proportional = pos - 2000;
Jeonghoon 100:0e44e944a19f 386
Jeonghoon 107:94134c6f90e8 387 int derivative = proportional - last_proportional;
Jeonghoon 100:0e44e944a19f 388 integral += proportional;
Jeonghoon 107:94134c6f90e8 389
Jeonghoon 100:0e44e944a19f 390 last_proportional = proportional;
Jeonghoon 100:0e44e944a19f 391
Jeonghoon 107:94134c6f90e8 392 int power_difference = proportional / motor.kp+ derivative * motor.kd;
Jeonghoon 107:94134c6f90e8 393
Jeonghoon 110:13f123179c26 394 const float std = 970.0;
Jeonghoon 107:94134c6f90e8 395
Jeonghoon 103:b417a6c65a6f 396 if(power_difference > motor.max)
Jeonghoon 103:b417a6c65a6f 397 power_difference = motor.max;
Jeonghoon 100:0e44e944a19f 398
Jeonghoon 103:b417a6c65a6f 399 if(power_difference < -motor.max)
Jeonghoon 103:b417a6c65a6f 400 power_difference = -motor.max;
Jeonghoon 100:0e44e944a19f 401
Jeonghoon 107:94134c6f90e8 402 // bot position: right
Jeonghoon 107:94134c6f90e8 403 if(power_difference > 15){ //0.1 + p_d/std
Jeonghoon 107:94134c6f90e8 404 motor.speed_r((motor.max + power_difference)/std);
Jeonghoon 107:94134c6f90e8 405 motor.speed_l((motor.max - power_difference)/std);
Jeonghoon 107:94134c6f90e8 406 }
Jeonghoon 107:94134c6f90e8 407 // bot position: left
Jeonghoon 107:94134c6f90e8 408 else if(power_difference < -15){
Jeonghoon 107:94134c6f90e8 409 motor.speed_r((motor.max + power_difference)/std);
Jeonghoon 107:94134c6f90e8 410 motor.speed_l((motor.max - power_difference)/std);
Jeonghoon 107:94134c6f90e8 411 }
Jeonghoon 107:94134c6f90e8 412 else{ //online
Jeonghoon 107:94134c6f90e8 413 motor.speed_l(motor.init_sp_l);
Jeonghoon 107:94134c6f90e8 414 motor.speed_r(motor.init_sp_r);
Jeonghoon 107:94134c6f90e8 415 }
Jeonghoon 102:e846a127af54 416
Jeonghoon 106:d14f340f1fe0 417 }
Jeonghoon 106:d14f340f1fe0 418