embedded / Mbed OS line_tracing

190605

Dependencies:   WS2812 RemoteIR PixelArray Adafruit_GFX

main.cpp@106:d14f340f1fe0, 2019-06-10 (annotated)

Committer:
Jeonghoon
Date:
Mon Jun 10 11:10:06 2019 +0000
Revision:
106:d14f340f1fe0
Parent:
104:94ed54c74466
Child:
107:94134c6f90e8
kp 0.1 kd 0 max 200 std 950 (1:15)

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