embedded / Mbed OS line_tracing

190605

Dependencies:   WS2812 RemoteIR PixelArray Adafruit_GFX

main.cpp@102:e846a127af54, 2019-06-07 (annotated)

Committer:
Jeonghoon
Date:
Fri Jun 07 14:50:44 2019 +0000
Revision:
102:e846a127af54
Parent:
101:efa2315d0312
Child:
103:b417a6c65a6f
20190607 bluetooth

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