embedded / Mbed OS line_tracing

190605

Dependencies:   WS2812 RemoteIR PixelArray Adafruit_GFX

main.cpp@99:d8123dfcf757, 2019-06-05 (annotated)

Committer:
jinyoung_KIL
Date:
Wed Jun 05 13:56:18 2019 +0000
Revision:
99:d8123dfcf757
Parent:
97:bad45e2dc7e1 
20190605

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 99:d8123dfcf757 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 99:d8123dfcf757 25 unsigned int min_sensor_values[NUM_SENSORS];
mbed_official 82:abf1b1785bd7 26
jinyoung_KIL 97:bad45e2dc7e1 27 unsigned int *calibratedMin;
jinyoung_KIL 99:d8123dfcf757 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 99:d8123dfcf757 42
jinyoung_KIL 99:d8123dfcf757 43
jinyoung_KIL 97:bad45e2dc7e1 44 int readLine(unsigned int *sensor_values);
jinyoung_KIL 97:bad45e2dc7e1 45 void tr_ready(void);
jinyoung_KIL 97:bad45e2dc7e1 46 void calibration(void);
jinyoung_KIL 97:bad45e2dc7e1 47 void init(void);
jinyoung_KIL 99:d8123dfcf757 48
jinyoung_KIL 99:d8123dfcf757 49 void tracing_pid(void);
jinyoung_KIL 97:bad45e2dc7e1 50 void set_ir_val(void);
jinyoung_KIL 97:bad45e2dc7e1 51 void get_pos(void);
jinyoung_KIL 97:bad45e2dc7e1 52 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
jinyoung_KIL 97:bad45e2dc7e1 53 void remote_ctrl(void);
jinyoung_KIL 97:bad45e2dc7e1 54 void oled_disp(void);
jinyoung_KIL 97:bad45e2dc7e1 55
mbed_official 82:abf1b1785bd7 56 int main()
jinyoung_KIL 99:d8123dfcf757 57 {
jinyoung_KIL 97:bad45e2dc7e1 58 pc.printf("start\r\n");
jinyoung_KIL 97:bad45e2dc7e1 59 spi.format(16, 0);
jinyoung_KIL 97:bad45e2dc7e1 60 spi.frequency(2000000);
jinyoung_KIL 97:bad45e2dc7e1 61
jinyoung_KIL 97:bad45e2dc7e1 62 init();
jinyoung_KIL 99:d8123dfcf757 63
jinyoung_KIL 97:bad45e2dc7e1 64 oled_thread.start(&oled_disp);
jinyoung_KIL 99:d8123dfcf757 65 remote_thread.start(&remote_ctrl);
jinyoung_KIL 99:d8123dfcf757 66 while(!motor.cal_start);
jinyoung_KIL 99:d8123dfcf757 67
jinyoung_KIL 97:bad45e2dc7e1 68 calibration();
jinyoung_KIL 99:d8123dfcf757 69
jinyoung_KIL 97:bad45e2dc7e1 70 while(!flag);
jinyoung_KIL 97:bad45e2dc7e1 71
jinyoung_KIL 97:bad45e2dc7e1 72 TR_thread.start(&tr_ready);
jinyoung_KIL 97:bad45e2dc7e1 73
jinyoung_KIL 99:d8123dfcf757 74
jinyoung_KIL 97:bad45e2dc7e1 75 while(!start);
jinyoung_KIL 99:d8123dfcf757 76
jinyoung_KIL 99:d8123dfcf757 77 pc.printf("motor forward\r\n");
jinyoung_KIL 97:bad45e2dc7e1 78 motor.forward();
jinyoung_KIL 99:d8123dfcf757 79
jinyoung_KIL 97:bad45e2dc7e1 80 while (1);
jinyoung_KIL 97:bad45e2dc7e1 81 }
jinyoung_KIL 97:bad45e2dc7e1 82
jinyoung_KIL 97:bad45e2dc7e1 83 void remote_ctrl(void){
jinyoung_KIL 99:d8123dfcf757 84
jinyoung_KIL 97:bad45e2dc7e1 85 while(1){
jinyoung_KIL 97:bad45e2dc7e1 86 uint8_t buf1[32];
jinyoung_KIL 97:bad45e2dc7e1 87 uint8_t buf2[32];
jinyoung_KIL 97:bad45e2dc7e1 88 int bitlength1;
jinyoung_KIL 97:bad45e2dc7e1 89 RemoteIR::Format format;
jinyoung_KIL 97:bad45e2dc7e1 90
jinyoung_KIL 97:bad45e2dc7e1 91 memset(buf1, 0x00, sizeof(buf1));
jinyoung_KIL 97:bad45e2dc7e1 92 memset(buf2, 0x00, sizeof(buf2));
jinyoung_KIL 97:bad45e2dc7e1 93
jinyoung_KIL 97:bad45e2dc7e1 94 bitlength1 = receive(&format, buf1, sizeof(buf1));
jinyoung_KIL 97:bad45e2dc7e1 95 if (bitlength1 < 0) {
jinyoung_KIL 97:bad45e2dc7e1 96 continue;
jinyoung_KIL 97:bad45e2dc7e1 97 }
jinyoung_KIL 97:bad45e2dc7e1 98
jinyoung_KIL 99:d8123dfcf757 99 }
jinyoung_KIL 97:bad45e2dc7e1 100 }
mbed_official 82:abf1b1785bd7 101
jinyoung_KIL 97:bad45e2dc7e1 102 void init(void){
jinyoung_KIL 99:d8123dfcf757 103 motor.cal_start = 0;
jinyoung_KIL 99:d8123dfcf757 104 motor.cal_stop = 0;
jinyoung_KIL 99:d8123dfcf757 105
jinyoung_KIL 99:d8123dfcf757 106 motor.kp = 10;
jinyoung_KIL 99:d8123dfcf757 107 motor.max = 130;
jinyoung_KIL 99:d8123dfcf757 108
jinyoung_KIL 97:bad45e2dc7e1 109 DataComm = 0;
jinyoung_KIL 99:d8123dfcf757 110
jinyoung_KIL 97:bad45e2dc7e1 111 calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
jinyoung_KIL 97:bad45e2dc7e1 112 calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
jinyoung_KIL 97:bad45e2dc7e1 113
jinyoung_KIL 97:bad45e2dc7e1 114 for(int i=0; i < NUM_SENSORS; i++)
jinyoung_KIL 97:bad45e2dc7e1 115 {
jinyoung_KIL 97:bad45e2dc7e1 116 calibratedMin[i] = 1023;
jinyoung_KIL 97:bad45e2dc7e1 117 calibratedMax[i] = 0;
jinyoung_KIL 97:bad45e2dc7e1 118 }
jinyoung_KIL 97:bad45e2dc7e1 119 }
jinyoung_KIL 97:bad45e2dc7e1 120
jinyoung_KIL 97:bad45e2dc7e1 121 void oled_disp(void){
jinyoung_KIL 97:bad45e2dc7e1 122 myOled.begin();
jinyoung_KIL 97:bad45e2dc7e1 123 while(1){
jinyoung_KIL 97:bad45e2dc7e1 124 myOled.clearDisplay();
jinyoung_KIL 99:d8123dfcf757 125 //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
jinyoung_KIL 99:d8123dfcf757 126 myOled.printf("kp:%.2f, max:%d\r", motor.kp, motor.max);
jinyoung_KIL 99:d8123dfcf757 127 myOled.display();
jinyoung_KIL 97:bad45e2dc7e1 128 }
jinyoung_KIL 97:bad45e2dc7e1 129 }
mbed_official 82:abf1b1785bd7 130
jinyoung_KIL 97:bad45e2dc7e1 131 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
jinyoung_KIL 97:bad45e2dc7e1 132 int cnt = 0;
jinyoung_KIL 97:bad45e2dc7e1 133 while (motor.getState() != ReceiverIR::Received) {
jinyoung_KIL 97:bad45e2dc7e1 134 cnt++;
jinyoung_KIL 97:bad45e2dc7e1 135 if (timeout < cnt) {
jinyoung_KIL 97:bad45e2dc7e1 136 return -1;
mbed_official 88:bea4f2daa48c 137 }
jinyoung_KIL 97:bad45e2dc7e1 138 }
jinyoung_KIL 97:bad45e2dc7e1 139 return motor.getData(format, buf, bufsiz * 8);
jinyoung_KIL 97:bad45e2dc7e1 140 }
jinyoung_KIL 97:bad45e2dc7e1 141
jinyoung_KIL 97:bad45e2dc7e1 142
jinyoung_KIL 97:bad45e2dc7e1 143 void read_ir(void){
jinyoung_KIL 97:bad45e2dc7e1 144 ch = 0;
jinyoung_KIL 99:d8123dfcf757 145
jinyoung_KIL 97:bad45e2dc7e1 146 spi_cs = 0;
jinyoung_KIL 97:bad45e2dc7e1 147 wait_us(2);
jinyoung_KIL 97:bad45e2dc7e1 148 value = spi.write(ch << 12);
jinyoung_KIL 97:bad45e2dc7e1 149 spi_cs = 1;
jinyoung_KIL 99:d8123dfcf757 150
jinyoung_KIL 99:d8123dfcf757 151 wait_us(21);
jinyoung_KIL 99:d8123dfcf757 152
jinyoung_KIL 97:bad45e2dc7e1 153 for(int i = 0; i < 5; i++){
jinyoung_KIL 99:d8123dfcf757 154
jinyoung_KIL 97:bad45e2dc7e1 155 ch += 1;
jinyoung_KIL 99:d8123dfcf757 156
jinyoung_KIL 97:bad45e2dc7e1 157 spi_cs = 0;
jinyoung_KIL 97:bad45e2dc7e1 158 wait_us(2);
jinyoung_KIL 97:bad45e2dc7e1 159 value = spi.write(ch << 12);
jinyoung_KIL 97:bad45e2dc7e1 160 spi_cs = 1;
jinyoung_KIL 99:d8123dfcf757 161
jinyoung_KIL 99:d8123dfcf757 162 wait_us(21);
jinyoung_KIL 99:d8123dfcf757 163
jinyoung_KIL 97:bad45e2dc7e1 164 value = value >> 6;
jinyoung_KIL 97:bad45e2dc7e1 165 value = value * 3300 / 0x3FF;
jinyoung_KIL 99:d8123dfcf757 166
jinyoung_KIL 97:bad45e2dc7e1 167 sensor_values[i] = value;
Jonathan Austin 0:2757d7abb7d9 168 }
Jonathan Austin 0:2757d7abb7d9 169 }
jinyoung_KIL 97:bad45e2dc7e1 170
jinyoung_KIL 97:bad45e2dc7e1 171 void calibration(void){
jinyoung_KIL 97:bad45e2dc7e1 172 pc.printf("calibration start \r\n");
jinyoung_KIL 97:bad45e2dc7e1 173
jinyoung_KIL 99:d8123dfcf757 174 int i = 0;
jinyoung_KIL 99:d8123dfcf757 175 while(!motor.cal_stop){
jinyoung_KIL 97:bad45e2dc7e1 176 read_ir();
jinyoung_KIL 97:bad45e2dc7e1 177 for(int j = 0; j < NUM_SENSORS; j++){
jinyoung_KIL 97:bad45e2dc7e1 178 if(i == 0 || max_sensor_values[j] < sensor_values[j]){
jinyoung_KIL 97:bad45e2dc7e1 179 max_sensor_values[j] = sensor_values[j];
jinyoung_KIL 97:bad45e2dc7e1 180 }
jinyoung_KIL 99:d8123dfcf757 181
jinyoung_KIL 97:bad45e2dc7e1 182 if(i == 0 || min_sensor_values[j] > sensor_values[j]){
jinyoung_KIL 97:bad45e2dc7e1 183 min_sensor_values[j] = sensor_values[j];
jinyoung_KIL 97:bad45e2dc7e1 184 }
jinyoung_KIL 97:bad45e2dc7e1 185 }
jinyoung_KIL 99:d8123dfcf757 186 i = 1;
jinyoung_KIL 97:bad45e2dc7e1 187 }
jinyoung_KIL 99:d8123dfcf757 188
jinyoung_KIL 97:bad45e2dc7e1 189 for(int i = 0; i < NUM_SENSORS; i++){
jinyoung_KIL 97:bad45e2dc7e1 190 if(min_sensor_values[i] < calibratedMin[i])
jinyoung_KIL 97:bad45e2dc7e1 191 calibratedMin[i] = min_sensor_values[i];
jinyoung_KIL 97:bad45e2dc7e1 192 if(max_sensor_values[i] > calibratedMax[i])
jinyoung_KIL 97:bad45e2dc7e1 193 calibratedMax[i] = max_sensor_values[i];
jinyoung_KIL 97:bad45e2dc7e1 194 }
jinyoung_KIL 99:d8123dfcf757 195
jinyoung_KIL 99:d8123dfcf757 196 pc.printf("calibration complete\r\n");
jinyoung_KIL 99:d8123dfcf757 197 flag = 1;
jinyoung_KIL 97:bad45e2dc7e1 198 }
jinyoung_KIL 97:bad45e2dc7e1 199
jinyoung_KIL 97:bad45e2dc7e1 200 void tr_ready(void){
jinyoung_KIL 99:d8123dfcf757 201
jinyoung_KIL 97:bad45e2dc7e1 202 while(1){
jinyoung_KIL 97:bad45e2dc7e1 203 // read current IR 1 ~ IR 5
jinyoung_KIL 97:bad45e2dc7e1 204 read_ir();
jinyoung_KIL 99:d8123dfcf757 205
jinyoung_KIL 97:bad45e2dc7e1 206 // set range under 1000
jinyoung_KIL 97:bad45e2dc7e1 207 set_ir_val();
jinyoung_KIL 99:d8123dfcf757 208
jinyoung_KIL 97:bad45e2dc7e1 209 // get current position
jinyoung_KIL 97:bad45e2dc7e1 210 get_pos();
jinyoung_KIL 99:d8123dfcf757 211
jinyoung_KIL 99:d8123dfcf757 212 tracing_pid();
jinyoung_KIL 97:bad45e2dc7e1 213
jinyoung_KIL 99:d8123dfcf757 214
jinyoung_KIL 99:d8123dfcf757 215 }
jinyoung_KIL 97:bad45e2dc7e1 216 }
jinyoung_KIL 97:bad45e2dc7e1 217
jinyoung_KIL 97:bad45e2dc7e1 218 void set_ir_val(){
jinyoung_KIL 99:d8123dfcf757 219 for(int i = 0; i < NUM_SENSORS; i++)
jinyoung_KIL 97:bad45e2dc7e1 220 {
jinyoung_KIL 97:bad45e2dc7e1 221 unsigned int denominator;
jinyoung_KIL 97:bad45e2dc7e1 222
jinyoung_KIL 97:bad45e2dc7e1 223 denominator = calibratedMax[i] - calibratedMin[i];
jinyoung_KIL 97:bad45e2dc7e1 224
jinyoung_KIL 97:bad45e2dc7e1 225 signed int x = 0;
jinyoung_KIL 97:bad45e2dc7e1 226 if(denominator != 0)
jinyoung_KIL 97:bad45e2dc7e1 227 x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
jinyoung_KIL 97:bad45e2dc7e1 228 if(x < 0) x = 0;
jinyoung_KIL 97:bad45e2dc7e1 229 else if(x > 1000) x = 1000;
jinyoung_KIL 99:d8123dfcf757 230
jinyoung_KIL 99:d8123dfcf757 231 sensor_values[i] = x;
jinyoung_KIL 97:bad45e2dc7e1 232
jinyoung_KIL 97:bad45e2dc7e1 233 }
jinyoung_KIL 99:d8123dfcf757 234
jinyoung_KIL 99:d8123dfcf757 235 // finish line
jinyoung_KIL 99:d8123dfcf757 236 if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
jinyoung_KIL 99:d8123dfcf757 237 motor.stop();
jinyoung_KIL 99:d8123dfcf757 238 wait(5);
jinyoung_KIL 99:d8123dfcf757 239 }
jinyoung_KIL 99:d8123dfcf757 240
jinyoung_KIL 99:d8123dfcf757 241 for(int i = 0; i < NUM_SENSORS; i++){
jinyoung_KIL 99:d8123dfcf757 242
jinyoung_KIL 99:d8123dfcf757 243 pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
jinyoung_KIL 99:d8123dfcf757 244 }
jinyoung_KIL 99:d8123dfcf757 245
jinyoung_KIL 99:d8123dfcf757 246 pc.printf("\r\n");
jinyoung_KIL 97:bad45e2dc7e1 247 }
jinyoung_KIL 97:bad45e2dc7e1 248
jinyoung_KIL 97:bad45e2dc7e1 249 void get_pos(){
jinyoung_KIL 97:bad45e2dc7e1 250 on_line = 0;
jinyoung_KIL 97:bad45e2dc7e1 251 avg = 0;
jinyoung_KIL 99:d8123dfcf757 252 sum = 0;
jinyoung_KIL 97:bad45e2dc7e1 253
jinyoung_KIL 97:bad45e2dc7e1 254 for(int i = 0; i < NUM_SENSORS; i++){
jinyoung_KIL 97:bad45e2dc7e1 255 int val = sensor_values[i];
jinyoung_KIL 99:d8123dfcf757 256
jinyoung_KIL 99:d8123dfcf757 257 // determine "on_line" or "out_line"
jinyoung_KIL 99:d8123dfcf757 258 if(val < 500){
jinyoung_KIL 97:bad45e2dc7e1 259 on_line = 1;
jinyoung_KIL 97:bad45e2dc7e1 260 }
jinyoung_KIL 99:d8123dfcf757 261
jinyoung_KIL 99:d8123dfcf757 262 // under
jinyoung_KIL 97:bad45e2dc7e1 263 if(val > 5){
jinyoung_KIL 97:bad45e2dc7e1 264 avg += (long)(val) * (i * 1000);
jinyoung_KIL 99:d8123dfcf757 265 sum += val;
jinyoung_KIL 99:d8123dfcf757 266 }
jinyoung_KIL 97:bad45e2dc7e1 267 }
jinyoung_KIL 99:d8123dfcf757 268
jinyoung_KIL 97:bad45e2dc7e1 269 // out_line
jinyoung_KIL 97:bad45e2dc7e1 270 if(!on_line){
jinyoung_KIL 99:d8123dfcf757 271 if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
jinyoung_KIL 97:bad45e2dc7e1 272 pos = 0; // last_vlaue = 0
jinyoung_KIL 99:d8123dfcf757 273 }
jinyoung_KIL 99:d8123dfcf757 274 else{ // right -> out-line (over 2000)
jinyoung_KIL 97:bad45e2dc7e1 275 pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
jinyoung_KIL 97:bad45e2dc7e1 276 }
jinyoung_KIL 97:bad45e2dc7e1 277 }
jinyoung_KIL 97:bad45e2dc7e1 278 // on_line
jinyoung_KIL 97:bad45e2dc7e1 279 else{
jinyoung_KIL 99:d8123dfcf757 280 pos = avg / sum;
jinyoung_KIL 97:bad45e2dc7e1 281 }
jinyoung_KIL 97:bad45e2dc7e1 282
jinyoung_KIL 99:d8123dfcf757 283 start = 1;
jinyoung_KIL 97:bad45e2dc7e1 284 }
jinyoung_KIL 97:bad45e2dc7e1 285
jinyoung_KIL 97:bad45e2dc7e1 286 void tracing_pid(void){
jinyoung_KIL 97:bad45e2dc7e1 287
jinyoung_KIL 99:d8123dfcf757 288
jinyoung_KIL 97:bad45e2dc7e1 289 int proportional = pos - 2000;
jinyoung_KIL 97:bad45e2dc7e1 290
jinyoung_KIL 97:bad45e2dc7e1 291 int derivative = proportional - last_proportional;
jinyoung_KIL 97:bad45e2dc7e1 292 integral += proportional;
jinyoung_KIL 99:d8123dfcf757 293
jinyoung_KIL 97:bad45e2dc7e1 294 last_proportional = proportional;
jinyoung_KIL 99:d8123dfcf757 295
jinyoung_KIL 99:d8123dfcf757 296 //int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
jinyoung_KIL 99:d8123dfcf757 297 int power_difference = proportional / motor.kp;
jinyoung_KIL 99:d8123dfcf757 298
jinyoung_KIL 99:d8123dfcf757 299 const float std = 1000;
jinyoung_KIL 99:d8123dfcf757 300
jinyoung_KIL 99:d8123dfcf757 301 if(power_difference > motor.max)
jinyoung_KIL 99:d8123dfcf757 302 power_difference = motor.max;
jinyoung_KIL 99:d8123dfcf757 303
jinyoung_KIL 99:d8123dfcf757 304 if(power_difference < -motor.max)
jinyoung_KIL 99:d8123dfcf757 305 power_difference = -motor.max;
jinyoung_KIL 97:bad45e2dc7e1 306
jinyoung_KIL 99:d8123dfcf757 307 // bot position: right
jinyoung_KIL 99:d8123dfcf757 308 if(power_difference > 15){
jinyoung_KIL 99:d8123dfcf757 309 motor.speed_r((motor.max + power_difference)/std);
jinyoung_KIL 99:d8123dfcf757 310 motor.speed_l(motor.max/33);
jinyoung_KIL 99:d8123dfcf757 311 }
jinyoung_KIL 99:d8123dfcf757 312 // bot position: left
jinyoung_KIL 99:d8123dfcf757 313 else if(power_difference < -15){
jinyoung_KIL 99:d8123dfcf757 314 motor.speed_r(motor.max/std);
jinyoung_KIL 99:d8123dfcf757 315 motor.speed_l((motor.max - power_difference)/std);
jinyoung_KIL 97:bad45e2dc7e1 316 }
jinyoung_KIL 99:d8123dfcf757 317 else{ //online
jinyoung_KIL 99:d8123dfcf757 318 motor.speed_l(motor.init_sp_l);
jinyoung_KIL 99:d8123dfcf757 319 motor.speed_r(motor.init_sp_r);
jinyoung_KIL 97:bad45e2dc7e1 320 }
jinyoung_KIL 99:d8123dfcf757 321
jinyoung_KIL 99:d8123dfcf757 322 // pc.printf("proportional: %d\r\n", proportional);
jinyoung_KIL 99:d8123dfcf757 323 //pc.printf("(max + power_difference): %d\r\n", motor.max + power_difference);
jinyoung_KIL 99:d8123dfcf757 324 // pc.printf("derivative: %d\r\n", derivative);
jinyoung_KIL 99:d8123dfcf757 325 // pc.printf("integral: %d\r\n",integral);
jinyoung_KIL 99:d8123dfcf757 326 }