embedded / Mbed OS line_tracing

190605

Dependencies:   WS2812 RemoteIR PixelArray Adafruit_GFX

main.cpp@97:bad45e2dc7e1, 2019-06-05 (annotated)

Committer:
jinyoung_KIL
Date:
Wed Jun 05 04:49:58 2019 +0000
Revision:
97:bad45e2dc7e1
Parent:
88:bea4f2daa48c
Child:
99:d8123dfcf757
Child:
100:0e44e944a19f
line_tracing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jinyoung_KIL 97:bad45e2dc7e1 1 #include "mbed.h"
jinyoung_KIL 97:bad45e2dc7e1 2 //#include "motor.h"
jinyoung_KIL 97:bad45e2dc7e1 3 #include "ReceiverIR.h"
jinyoung_KIL 97:bad45e2dc7e1 4 #include "Adafruit_SSD1306.h" // OLED
jinyoung_KIL 97:bad45e2dc7e1 5 #define NUM_SENSORS 5
jinyoung_KIL 97:bad45e2dc7e1 6
jinyoung_KIL 97:bad45e2dc7e1 7 //Motor motor(D6, D5, PA_0, PA_1, PB_0, PA_4, 0.3);
jinyoung_KIL 97:bad45e2dc7e1 8 ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4);
jinyoung_KIL 97:bad45e2dc7e1 9 SPI spi(D11, D12, D13);
jinyoung_KIL 97:bad45e2dc7e1 10 DigitalOut spi_cs(D10, 1);
jinyoung_KIL 97:bad45e2dc7e1 11 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
jinyoung_KIL 97:bad45e2dc7e1 12 I2C i2c(D14, D15); // D14, D15
jinyoung_KIL 97:bad45e2dc7e1 13 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); // D9, (D8: D/C - data/command change)
jinyoung_KIL 97:bad45e2dc7e1 14 DigitalOut DataComm(D8);
jinyoung_KIL 97:bad45e2dc7e1 15
jinyoung_KIL 97:bad45e2dc7e1 16 Thread TR_thread;
jinyoung_KIL 97:bad45e2dc7e1 17 Thread remote_thread;
jinyoung_KIL 97:bad45e2dc7e1 18 Thread oled_thread;
jinyoung_KIL 97:bad45e2dc7e1 19
jinyoung_KIL 97:bad45e2dc7e1 20 unsigned long avg = 0;
jinyoung_KIL 97:bad45e2dc7e1 21 unsigned int sum = 0;
jinyoung_KIL 97:bad45e2dc7e1 22
jinyoung_KIL 97:bad45e2dc7e1 23 unsigned int sensor_values[NUM_SENSORS];
jinyoung_KIL 97:bad45e2dc7e1 24 unsigned int max_sensor_values[NUM_SENSORS];
jinyoung_KIL 97:bad45e2dc7e1 25 unsigned int min_sensor_values[NUM_SENSORS];
mbed_official 82:abf1b1785bd7 26
jinyoung_KIL 97:bad45e2dc7e1 27 unsigned int *calibratedMin;
jinyoung_KIL 97:bad45e2dc7e1 28 unsigned int *calibratedMax;
Jonathan Austin 0:2757d7abb7d9 29
jinyoung_KIL 97:bad45e2dc7e1 30 int value;
jinyoung_KIL 97:bad45e2dc7e1 31 float bat;
jinyoung_KIL 97:bad45e2dc7e1 32 int on_line = 0;
jinyoung_KIL 97:bad45e2dc7e1 33 static int pos = 0;
jinyoung_KIL 97:bad45e2dc7e1 34 unsigned int last_proportional = 0;
jinyoung_KIL 97:bad45e2dc7e1 35 long integral = 0;
jinyoung_KIL 97:bad45e2dc7e1 36
jinyoung_KIL 97:bad45e2dc7e1 37 volatile int flag = 0;
jinyoung_KIL 97:bad45e2dc7e1 38 volatile int start = 0;
Jonathan Austin 0:2757d7abb7d9 39
jinyoung_KIL 97:bad45e2dc7e1 40 int ch;
mbed_official 88:bea4f2daa48c 41
jinyoung_KIL 97:bad45e2dc7e1 42 int readLine(unsigned int *sensor_values);
jinyoung_KIL 97:bad45e2dc7e1 43 void tr_ready(void);
jinyoung_KIL 97:bad45e2dc7e1 44 void calibration(void);
jinyoung_KIL 97:bad45e2dc7e1 45 void init(void);
jinyoung_KIL 97:bad45e2dc7e1 46 void tracing(void);
jinyoung_KIL 97:bad45e2dc7e1 47 //void tracing_pid(void);
jinyoung_KIL 97:bad45e2dc7e1 48 void set_ir_val(void);
jinyoung_KIL 97:bad45e2dc7e1 49 void get_pos(void);
jinyoung_KIL 97:bad45e2dc7e1 50 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
jinyoung_KIL 97:bad45e2dc7e1 51 void remote_ctrl(void);
jinyoung_KIL 97:bad45e2dc7e1 52 void oled_disp(void);
jinyoung_KIL 97:bad45e2dc7e1 53
mbed_official 82:abf1b1785bd7 54 int main()
jinyoung_KIL 97:bad45e2dc7e1 55 {
jinyoung_KIL 97:bad45e2dc7e1 56 pc.printf("start\r\n");
jinyoung_KIL 97:bad45e2dc7e1 57 spi.format(16, 0);
jinyoung_KIL 97:bad45e2dc7e1 58 spi.frequency(2000000);
jinyoung_KIL 97:bad45e2dc7e1 59
jinyoung_KIL 97:bad45e2dc7e1 60 init();
jinyoung_KIL 97:bad45e2dc7e1 61
jinyoung_KIL 97:bad45e2dc7e1 62 oled_thread.start(&oled_disp);
jinyoung_KIL 97:bad45e2dc7e1 63 remote_thread.start(&remote_ctrl);
jinyoung_KIL 97:bad45e2dc7e1 64 while(!motor.start);
jinyoung_KIL 97:bad45e2dc7e1 65
jinyoung_KIL 97:bad45e2dc7e1 66 calibration();
jinyoung_KIL 97:bad45e2dc7e1 67
jinyoung_KIL 97:bad45e2dc7e1 68 while(!flag);
jinyoung_KIL 97:bad45e2dc7e1 69
jinyoung_KIL 97:bad45e2dc7e1 70 TR_thread.start(&tr_ready);
jinyoung_KIL 97:bad45e2dc7e1 71
jinyoung_KIL 97:bad45e2dc7e1 72
jinyoung_KIL 97:bad45e2dc7e1 73 while(!start);
jinyoung_KIL 97:bad45e2dc7e1 74
jinyoung_KIL 97:bad45e2dc7e1 75 pc.printf("motor forward\r\n");
jinyoung_KIL 97:bad45e2dc7e1 76 motor.forward();
jinyoung_KIL 97:bad45e2dc7e1 77
jinyoung_KIL 97:bad45e2dc7e1 78 while (1);
jinyoung_KIL 97:bad45e2dc7e1 79 }
jinyoung_KIL 97:bad45e2dc7e1 80
jinyoung_KIL 97:bad45e2dc7e1 81 void remote_ctrl(void){
jinyoung_KIL 97:bad45e2dc7e1 82
jinyoung_KIL 97:bad45e2dc7e1 83 while(1){
jinyoung_KIL 97:bad45e2dc7e1 84 uint8_t buf1[32];
jinyoung_KIL 97:bad45e2dc7e1 85 uint8_t buf2[32];
jinyoung_KIL 97:bad45e2dc7e1 86 int bitlength1;
jinyoung_KIL 97:bad45e2dc7e1 87 RemoteIR::Format format;
jinyoung_KIL 97:bad45e2dc7e1 88
jinyoung_KIL 97:bad45e2dc7e1 89 memset(buf1, 0x00, sizeof(buf1));
jinyoung_KIL 97:bad45e2dc7e1 90 memset(buf2, 0x00, sizeof(buf2));
jinyoung_KIL 97:bad45e2dc7e1 91
jinyoung_KIL 97:bad45e2dc7e1 92 bitlength1 = receive(&format, buf1, sizeof(buf1));
jinyoung_KIL 97:bad45e2dc7e1 93 if (bitlength1 < 0) {
jinyoung_KIL 97:bad45e2dc7e1 94 continue;
jinyoung_KIL 97:bad45e2dc7e1 95 }
jinyoung_KIL 97:bad45e2dc7e1 96
jinyoung_KIL 97:bad45e2dc7e1 97 //display_status("RECV", bitlength1);
jinyoung_KIL 97:bad45e2dc7e1 98 //display_data(buf1, bitlength1);
jinyoung_KIL 97:bad45e2dc7e1 99 //display_format(format);
jinyoung_KIL 97:bad45e2dc7e1 100
jinyoung_KIL 97:bad45e2dc7e1 101 }
jinyoung_KIL 97:bad45e2dc7e1 102 }
mbed_official 82:abf1b1785bd7 103
jinyoung_KIL 97:bad45e2dc7e1 104 void init(void){
jinyoung_KIL 97:bad45e2dc7e1 105 DataComm = 0;
jinyoung_KIL 97:bad45e2dc7e1 106
jinyoung_KIL 97:bad45e2dc7e1 107 calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
jinyoung_KIL 97:bad45e2dc7e1 108 calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
jinyoung_KIL 97:bad45e2dc7e1 109
jinyoung_KIL 97:bad45e2dc7e1 110 for(int i=0; i < NUM_SENSORS; i++)
jinyoung_KIL 97:bad45e2dc7e1 111 {
jinyoung_KIL 97:bad45e2dc7e1 112 calibratedMin[i] = 1023;
jinyoung_KIL 97:bad45e2dc7e1 113 calibratedMax[i] = 0;
jinyoung_KIL 97:bad45e2dc7e1 114 }
jinyoung_KIL 97:bad45e2dc7e1 115 }
jinyoung_KIL 97:bad45e2dc7e1 116
jinyoung_KIL 97:bad45e2dc7e1 117 void oled_disp(void){
jinyoung_KIL 97:bad45e2dc7e1 118 myOled.begin();
jinyoung_KIL 97:bad45e2dc7e1 119 while(1){
jinyoung_KIL 97:bad45e2dc7e1 120 myOled.clearDisplay();
jinyoung_KIL 97:bad45e2dc7e1 121 myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
jinyoung_KIL 97:bad45e2dc7e1 122 myOled.display();
jinyoung_KIL 97:bad45e2dc7e1 123 }
jinyoung_KIL 97:bad45e2dc7e1 124 }
mbed_official 82:abf1b1785bd7 125
jinyoung_KIL 97:bad45e2dc7e1 126 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
jinyoung_KIL 97:bad45e2dc7e1 127 int cnt = 0;
jinyoung_KIL 97:bad45e2dc7e1 128 while (motor.getState() != ReceiverIR::Received) {
jinyoung_KIL 97:bad45e2dc7e1 129 cnt++;
jinyoung_KIL 97:bad45e2dc7e1 130 if (timeout < cnt) {
jinyoung_KIL 97:bad45e2dc7e1 131 return -1;
mbed_official 88:bea4f2daa48c 132 }
jinyoung_KIL 97:bad45e2dc7e1 133 }
jinyoung_KIL 97:bad45e2dc7e1 134 return motor.getData(format, buf, bufsiz * 8);
jinyoung_KIL 97:bad45e2dc7e1 135 }
jinyoung_KIL 97:bad45e2dc7e1 136
jinyoung_KIL 97:bad45e2dc7e1 137
jinyoung_KIL 97:bad45e2dc7e1 138 void read_ir(void){
jinyoung_KIL 97:bad45e2dc7e1 139 ch = 0;
jinyoung_KIL 97:bad45e2dc7e1 140
jinyoung_KIL 97:bad45e2dc7e1 141 spi_cs = 0;
jinyoung_KIL 97:bad45e2dc7e1 142 wait_us(2);
jinyoung_KIL 97:bad45e2dc7e1 143 value = spi.write(ch << 12);
jinyoung_KIL 97:bad45e2dc7e1 144 spi_cs = 1;
jinyoung_KIL 97:bad45e2dc7e1 145
jinyoung_KIL 97:bad45e2dc7e1 146 wait_us(21);
jinyoung_KIL 97:bad45e2dc7e1 147
jinyoung_KIL 97:bad45e2dc7e1 148 for(int i = 0; i < 5; i++){
jinyoung_KIL 97:bad45e2dc7e1 149
jinyoung_KIL 97:bad45e2dc7e1 150 ch += 1;
jinyoung_KIL 97:bad45e2dc7e1 151
jinyoung_KIL 97:bad45e2dc7e1 152 spi_cs = 0;
jinyoung_KIL 97:bad45e2dc7e1 153 wait_us(2);
jinyoung_KIL 97:bad45e2dc7e1 154 value = spi.write(ch << 12);
jinyoung_KIL 97:bad45e2dc7e1 155 spi_cs = 1;
jinyoung_KIL 97:bad45e2dc7e1 156
jinyoung_KIL 97:bad45e2dc7e1 157 wait_us(21);
jinyoung_KIL 97:bad45e2dc7e1 158
jinyoung_KIL 97:bad45e2dc7e1 159 value = value >> 6;
jinyoung_KIL 97:bad45e2dc7e1 160 value = value * 3300 / 0x3FF;
jinyoung_KIL 97:bad45e2dc7e1 161
jinyoung_KIL 97:bad45e2dc7e1 162 sensor_values[i] = value;
Jonathan Austin 0:2757d7abb7d9 163 }
Jonathan Austin 0:2757d7abb7d9 164 }
jinyoung_KIL 97:bad45e2dc7e1 165
jinyoung_KIL 97:bad45e2dc7e1 166 void calibration(void){
jinyoung_KIL 97:bad45e2dc7e1 167 pc.printf("calibration start \r\n");
jinyoung_KIL 97:bad45e2dc7e1 168
jinyoung_KIL 97:bad45e2dc7e1 169 for(int i = 0; i < 10; i++){
jinyoung_KIL 97:bad45e2dc7e1 170 read_ir();
jinyoung_KIL 97:bad45e2dc7e1 171 for(int j = 0; j < NUM_SENSORS; j++){
jinyoung_KIL 97:bad45e2dc7e1 172 if(i == 0 || max_sensor_values[j] < sensor_values[j]){
jinyoung_KIL 97:bad45e2dc7e1 173 max_sensor_values[j] = sensor_values[j];
jinyoung_KIL 97:bad45e2dc7e1 174 }
jinyoung_KIL 97:bad45e2dc7e1 175
jinyoung_KIL 97:bad45e2dc7e1 176 if(i == 0 || min_sensor_values[j] > sensor_values[j]){
jinyoung_KIL 97:bad45e2dc7e1 177 min_sensor_values[j] = sensor_values[j];
jinyoung_KIL 97:bad45e2dc7e1 178 }
jinyoung_KIL 97:bad45e2dc7e1 179 }
jinyoung_KIL 97:bad45e2dc7e1 180 wait(0.5);
jinyoung_KIL 97:bad45e2dc7e1 181 }
jinyoung_KIL 97:bad45e2dc7e1 182
jinyoung_KIL 97:bad45e2dc7e1 183 for(int i = 0; i < NUM_SENSORS; i++){
jinyoung_KIL 97:bad45e2dc7e1 184 if(min_sensor_values[i] < calibratedMin[i])
jinyoung_KIL 97:bad45e2dc7e1 185 calibratedMin[i] = min_sensor_values[i];
jinyoung_KIL 97:bad45e2dc7e1 186 if(max_sensor_values[i] > calibratedMax[i])
jinyoung_KIL 97:bad45e2dc7e1 187 calibratedMax[i] = max_sensor_values[i];
jinyoung_KIL 97:bad45e2dc7e1 188 }
jinyoung_KIL 97:bad45e2dc7e1 189 pc.printf("calibration complete\r\n");
jinyoung_KIL 97:bad45e2dc7e1 190 flag =1;
jinyoung_KIL 97:bad45e2dc7e1 191 }
jinyoung_KIL 97:bad45e2dc7e1 192
jinyoung_KIL 97:bad45e2dc7e1 193 void tr_ready(void){
jinyoung_KIL 97:bad45e2dc7e1 194
jinyoung_KIL 97:bad45e2dc7e1 195 while(1){
jinyoung_KIL 97:bad45e2dc7e1 196 // read current IR 1 ~ IR 5
jinyoung_KIL 97:bad45e2dc7e1 197 read_ir();
jinyoung_KIL 97:bad45e2dc7e1 198
jinyoung_KIL 97:bad45e2dc7e1 199 // set range under 1000
jinyoung_KIL 97:bad45e2dc7e1 200 set_ir_val();
jinyoung_KIL 97:bad45e2dc7e1 201
jinyoung_KIL 97:bad45e2dc7e1 202 // get current position
jinyoung_KIL 97:bad45e2dc7e1 203 get_pos();
jinyoung_KIL 97:bad45e2dc7e1 204
jinyoung_KIL 97:bad45e2dc7e1 205 // pc.printf("pos = %d\r\n", pos);
jinyoung_KIL 97:bad45e2dc7e1 206 // start tracing_pid_pid
jinyoung_KIL 97:bad45e2dc7e1 207 //tracing_pid();
jinyoung_KIL 97:bad45e2dc7e1 208 tracing();
jinyoung_KIL 97:bad45e2dc7e1 209 // pc.printf("pos = %d\r\n", pos);
jinyoung_KIL 97:bad45e2dc7e1 210 // pc.printf("on_line = %d\r\n", on_line);
jinyoung_KIL 97:bad45e2dc7e1 211
jinyoung_KIL 97:bad45e2dc7e1 212 // wait(1);
jinyoung_KIL 97:bad45e2dc7e1 213 }
jinyoung_KIL 97:bad45e2dc7e1 214 }
jinyoung_KIL 97:bad45e2dc7e1 215
jinyoung_KIL 97:bad45e2dc7e1 216 void set_ir_val(){
jinyoung_KIL 97:bad45e2dc7e1 217 for(int i=0; i < NUM_SENSORS; i++)
jinyoung_KIL 97:bad45e2dc7e1 218 {
jinyoung_KIL 97:bad45e2dc7e1 219 unsigned int denominator;
jinyoung_KIL 97:bad45e2dc7e1 220
jinyoung_KIL 97:bad45e2dc7e1 221 denominator = calibratedMax[i] - calibratedMin[i];
jinyoung_KIL 97:bad45e2dc7e1 222
jinyoung_KIL 97:bad45e2dc7e1 223 signed int x = 0;
jinyoung_KIL 97:bad45e2dc7e1 224 if(denominator != 0)
jinyoung_KIL 97:bad45e2dc7e1 225 x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
jinyoung_KIL 97:bad45e2dc7e1 226 if(x < 0) x = 0;
jinyoung_KIL 97:bad45e2dc7e1 227 else if(x > 1000) x = 1000;
jinyoung_KIL 97:bad45e2dc7e1 228
jinyoung_KIL 97:bad45e2dc7e1 229 sensor_values[i] = x;
jinyoung_KIL 97:bad45e2dc7e1 230 }
jinyoung_KIL 97:bad45e2dc7e1 231 // for(int i = 0; i <NUM_SENSORS; i++){
jinyoung_KIL 97:bad45e2dc7e1 232 // pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
jinyoung_KIL 97:bad45e2dc7e1 233 // }
jinyoung_KIL 97:bad45e2dc7e1 234 }
jinyoung_KIL 97:bad45e2dc7e1 235
jinyoung_KIL 97:bad45e2dc7e1 236 void get_pos(){
jinyoung_KIL 97:bad45e2dc7e1 237 on_line = 0;
jinyoung_KIL 97:bad45e2dc7e1 238 avg = 0;
jinyoung_KIL 97:bad45e2dc7e1 239 sum = 0;
jinyoung_KIL 97:bad45e2dc7e1 240
jinyoung_KIL 97:bad45e2dc7e1 241 for(int i = 0; i < NUM_SENSORS; i++){
jinyoung_KIL 97:bad45e2dc7e1 242 int val = sensor_values[i];
jinyoung_KIL 97:bad45e2dc7e1 243
jinyoung_KIL 97:bad45e2dc7e1 244 // determine "on_line" or "out_line"
jinyoung_KIL 97:bad45e2dc7e1 245 if(val < 800){
jinyoung_KIL 97:bad45e2dc7e1 246 on_line = 1;
jinyoung_KIL 97:bad45e2dc7e1 247 }
jinyoung_KIL 97:bad45e2dc7e1 248
jinyoung_KIL 97:bad45e2dc7e1 249 // under
jinyoung_KIL 97:bad45e2dc7e1 250 if(val > 5){
jinyoung_KIL 97:bad45e2dc7e1 251 avg += (long)(val) * (i * 1000);
jinyoung_KIL 97:bad45e2dc7e1 252 sum += val;
jinyoung_KIL 97:bad45e2dc7e1 253 }
jinyoung_KIL 97:bad45e2dc7e1 254 }
jinyoung_KIL 97:bad45e2dc7e1 255
jinyoung_KIL 97:bad45e2dc7e1 256 // out_line
jinyoung_KIL 97:bad45e2dc7e1 257 if(!on_line){
jinyoung_KIL 97:bad45e2dc7e1 258 if(pos < (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
jinyoung_KIL 97:bad45e2dc7e1 259 pos = 0; // last_vlaue = 0
jinyoung_KIL 97:bad45e2dc7e1 260 }
jinyoung_KIL 97:bad45e2dc7e1 261 else{ // right -> out-line (over 2000)
jinyoung_KIL 97:bad45e2dc7e1 262 pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
jinyoung_KIL 97:bad45e2dc7e1 263 }
jinyoung_KIL 97:bad45e2dc7e1 264 }
jinyoung_KIL 97:bad45e2dc7e1 265 // on_line
jinyoung_KIL 97:bad45e2dc7e1 266 else{
jinyoung_KIL 97:bad45e2dc7e1 267 pos = avg / sum;
jinyoung_KIL 97:bad45e2dc7e1 268 }
jinyoung_KIL 97:bad45e2dc7e1 269 pc.printf("position: %d\r\n", pos);
jinyoung_KIL 97:bad45e2dc7e1 270 start = 1;
jinyoung_KIL 97:bad45e2dc7e1 271 }
jinyoung_KIL 97:bad45e2dc7e1 272
jinyoung_KIL 97:bad45e2dc7e1 273 void tracing(){
jinyoung_KIL 97:bad45e2dc7e1 274 // totally right
jinyoung_KIL 97:bad45e2dc7e1 275 if(pos == 4000){
jinyoung_KIL 97:bad45e2dc7e1 276 motor.speedup_r(3);
jinyoung_KIL 97:bad45e2dc7e1 277 }
jinyoung_KIL 97:bad45e2dc7e1 278 // detect IR1 on line
jinyoung_KIL 97:bad45e2dc7e1 279 else if(pos >= 2300){
jinyoung_KIL 97:bad45e2dc7e1 280 motor.speedup_r(3);
jinyoung_KIL 97:bad45e2dc7e1 281 }
jinyoung_KIL 97:bad45e2dc7e1 282 // detect IR2 on line
jinyoung_KIL 97:bad45e2dc7e1 283 else if(pos >= 2100){
jinyoung_KIL 97:bad45e2dc7e1 284 motor.speedup_r(1);
jinyoung_KIL 97:bad45e2dc7e1 285 }
jinyoung_KIL 97:bad45e2dc7e1 286 // detect IR3 on line
jinyoung_KIL 97:bad45e2dc7e1 287 else if(pos >= 1900){
jinyoung_KIL 97:bad45e2dc7e1 288 motor.speed_r(0.09);
jinyoung_KIL 97:bad45e2dc7e1 289 motor.speed_l(0.085);
jinyoung_KIL 97:bad45e2dc7e1 290 }
jinyoung_KIL 97:bad45e2dc7e1 291 // detect IR4 on line
jinyoung_KIL 97:bad45e2dc7e1 292 else if(pos >= 1700){
jinyoung_KIL 97:bad45e2dc7e1 293 motor.speedup_l(1);
jinyoung_KIL 97:bad45e2dc7e1 294 }
jinyoung_KIL 97:bad45e2dc7e1 295 // detect IR5 on line
jinyoung_KIL 97:bad45e2dc7e1 296 else if(pos >= 1200){
jinyoung_KIL 97:bad45e2dc7e1 297 motor.speedup_l(3);
jinyoung_KIL 97:bad45e2dc7e1 298 }
jinyoung_KIL 97:bad45e2dc7e1 299 // totally left
jinyoung_KIL 97:bad45e2dc7e1 300 else if(pos == 0){
jinyoung_KIL 97:bad45e2dc7e1 301 motor.speedup_l(3);
jinyoung_KIL 97:bad45e2dc7e1 302 }
jinyoung_KIL 97:bad45e2dc7e1 303 else {
jinyoung_KIL 97:bad45e2dc7e1 304 pc.printf("error pos: %d\r\n", pos);
jinyoung_KIL 97:bad45e2dc7e1 305 pc.printf("error\r\n");
jinyoung_KIL 97:bad45e2dc7e1 306 }
jinyoung_KIL 97:bad45e2dc7e1 307 wait(0.001);
jinyoung_KIL 97:bad45e2dc7e1 308
jinyoung_KIL 97:bad45e2dc7e1 309 }
jinyoung_KIL 97:bad45e2dc7e1 310
jinyoung_KIL 97:bad45e2dc7e1 311 void tracing_pid(void){
jinyoung_KIL 97:bad45e2dc7e1 312
jinyoung_KIL 97:bad45e2dc7e1 313 int proportional = pos - 2000;
jinyoung_KIL 97:bad45e2dc7e1 314
jinyoung_KIL 97:bad45e2dc7e1 315 int derivative = proportional - last_proportional;
jinyoung_KIL 97:bad45e2dc7e1 316 integral += proportional;
jinyoung_KIL 97:bad45e2dc7e1 317
jinyoung_KIL 97:bad45e2dc7e1 318 last_proportional = proportional;
jinyoung_KIL 97:bad45e2dc7e1 319
jinyoung_KIL 97:bad45e2dc7e1 320 int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
jinyoung_KIL 97:bad45e2dc7e1 321 // int power_difference = proportional / ;
jinyoung_KIL 97:bad45e2dc7e1 322
jinyoung_KIL 97:bad45e2dc7e1 323 const int max = 150;
jinyoung_KIL 97:bad45e2dc7e1 324
jinyoung_KIL 97:bad45e2dc7e1 325 if(power_difference > max)
jinyoung_KIL 97:bad45e2dc7e1 326 power_difference = max;
jinyoung_KIL 97:bad45e2dc7e1 327
jinyoung_KIL 97:bad45e2dc7e1 328 if(power_difference < -max)
jinyoung_KIL 97:bad45e2dc7e1 329 power_difference = -max;
jinyoung_KIL 97:bad45e2dc7e1 330
jinyoung_KIL 97:bad45e2dc7e1 331 if(power_difference < 0){
jinyoung_KIL 97:bad45e2dc7e1 332 motor.speed_l((max + power_difference)/255);
jinyoung_KIL 97:bad45e2dc7e1 333 motor.speed_r(max/255);
jinyoung_KIL 97:bad45e2dc7e1 334 }
jinyoung_KIL 97:bad45e2dc7e1 335 else{
jinyoung_KIL 97:bad45e2dc7e1 336 motor.speed_l(max/255);
jinyoung_KIL 97:bad45e2dc7e1 337 motor.speed_r((max - power_difference)/255);
jinyoung_KIL 97:bad45e2dc7e1 338 }
jinyoung_KIL 97:bad45e2dc7e1 339
jinyoung_KIL 97:bad45e2dc7e1 340 pc.printf("proportional: %d\r\n", proportional);
jinyoung_KIL 97:bad45e2dc7e1 341 pc.printf("power_difference: %d\r\n", power_difference);
jinyoung_KIL 97:bad45e2dc7e1 342 pc.printf("derivative: %d\r\n", derivative);
jinyoung_KIL 97:bad45e2dc7e1 343 pc.printf("integral: %d\r\n",integral);
jinyoung_KIL 97:bad45e2dc7e1 344 }