![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Everything put together
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Diff: main.cpp
- Revision:
- 4:947c3634b649
- Parent:
- 3:c57674c348bd
- Child:
- 5:7cba3ffc38bb
--- a/main.cpp Wed Mar 30 01:21:39 2016 +0000 +++ b/main.cpp Fri Apr 01 00:34:18 2016 +0000 @@ -22,7 +22,7 @@ //int idx = 0; char cmd; // Command char ch; -char in[2]; +char in[5]; void communication(void const *args); // Communications @@ -43,15 +43,17 @@ int prev_pulses = 0; // Previous pulse count int curr_pulses = 0; // Current pulse count float velocity = 0; // Velocity +float vel = 0; float v_prev[MVG_AVG] = {0}; // ======== // Velocity // ======== -float Kp = 13.0; // Proportional factor +float Kp = 6.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor float interval = 0.01; // Sampling interval +float ref_v = 1.0; PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller // ===== @@ -59,6 +61,7 @@ // ===== Servo servo(PTA12); // Enable pin (PWM) float a = 88; // Angle +float Ks = 0.7; // ====== // Camera @@ -81,7 +84,7 @@ int argmax = 0; int argmin = 0; int temp[128]; -int center; +int center = 64; void track(); // Line-tracking steering @@ -97,39 +100,72 @@ bt.printf(" [0] Velocity\r\n"); bt.printf(" [1] Steering\r\n"); bt.printf(" [2] Change Kp\r\n"); - bt.printf(" [3] Change exposure time\r\n"); + bt.printf(" [3] Change Ki\r\n"); + bt.printf(" [4] Change Kd\r\n"); + bt.printf(" [5] Change Ks\r\n"); + bt.printf(" [6] Change reference velocity\r\n"); cmd = bt.getc(); while (cmd != 'q') { switch(atoi(&cmd)){ case 0: - bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f\r\n", d, curr_pulses, velocity, Kp); + bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", d, curr_pulses, velocity, Kp, Ki, Kd); break; case 1: bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int); break; case 2: -// idx = 0; - bt.printf("Current Kp = %f. Enter new Kp: ", Kp); -// while ((cmd = bt.getc()) != '\n') { - for (int idx = 0; idx < 2; idx++) { - ch = bt.getc(); - in[idx] = ch; -// idx = idx + 1; + bt.printf("Current: %f, New (5 digits): ", Kp); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); } bt.printf("\r\n"); Kp = atof(in); + motor_ctrl.setTunings(Kp, Ki, Kd); + cmd = 'q'; break; case 3: -// idx = 0; - bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000); -// while ((cmd = bt.getc()) != '\n') { - for (int idx = 0; idx < 2; idx++) { - ch = bt.getc(); - in[idx] = ch; -// idx = idx + 1; + bt.printf("Current: %f, New (5 digits): ", Ki); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + Ki = atof(in); + motor_ctrl.setTunings(Kp, Ki, Kd); + cmd = 'q'; + break; + case 4: + bt.printf("Current: %f, New (5 digits): ", Kd); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); } bt.printf("\r\n"); - t_int = atoi(in); + Kd = atof(in); + motor_ctrl.setTunings(Kp, Ki, Kd); + cmd = 'q'; + break; + case 5: + bt.printf("Current: %f, New (5 digits): ", Ks); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + Ks = atof(in); + cmd = 'q'; + break; + case 6: + bt.printf("Current: %f, New (5 digits): ", ref_v); + for (int i = 0; i < 5; i++) { + in[i] = bt.getc(); + bt.putc(in[i]); + } + bt.printf("\r\n"); + ref_v = atof(in); + motor_ctrl.setSetPoint(ref_v); + cmd = 'q'; break; } if (bt.readable()) { @@ -200,26 +236,26 @@ } } - for (int i = 0; i < 30; i++) { - lum_bg = lum_bg + img[64 - 10 - i] / 60.0 + img[64 + 10 + i] / 60.0; + for (int i = 0; i < 10; i++) { + lum_bg = lum_bg + img[64 - 4 - i] / 20.0 + img[64 + 4 + i] / 20.0; } contrast = (max - lum_bg) / lum_bg; - if (contrast < 1.5) { +// if (contrast < 1.5) { // Underexposed - if (max < 55000) { - t_int = t_int + (55000 - max); + if (max < 60000) { + t_int = t_int + 0.15 * (60000 - max); } // Overexposed if (lum_bg > 25000) { - t_int = t_int - (lum_bg - 25000); + t_int = t_int - 0.15 * (lum_bg - 25000); } - } +// } if (max > 43253) { center = (argmax + argmin + 2 + 11) / 2; - a = 88 + (64 - center) * 0.625; + a = 88 + (64 - center) * Ks; servo = a / 180; } @@ -245,8 +281,8 @@ // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); - motor_ctrl.setOutputLimits(0.1, 0.5); - motor_ctrl.setSetPoint(1.5); + motor_ctrl.setOutputLimits(0.01, 0.5); + motor_ctrl.setSetPoint(ref_v); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); @@ -259,20 +295,21 @@ // Start motor controller while (true) { curr_pulses = qei.getPulses(); - for (int i = 0; i < MVG_AVG - 1; i++) { - v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1])); - } - v_prev[MVG_AVG-1] = velocity; - velocity = (curr_pulses - prev_pulses)/ interval / ppr * c; - for (int i = 0; i < MVG_AVG; i++) { - velocity = velocity + v_prev[i]; - } - velocity = velocity / (MVG_AVG + 1.0); + //for (int i = 0; i < MVG_AVG - 1; i++) { +// v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1])); +// } +// v_prev[MVG_AVG-1] = velocity; + velocity = (curr_pulses - prev_pulses)/ interval / ppr * c / 2.5; + //vel = velocity; +// for (int i = 0; i < MVG_AVG; i++) { +// velocity = velocity + v_prev[i]; +// } +// velocity = velocity / (MVG_AVG + 1.0); prev_pulses = curr_pulses; motor_ctrl.setProcessValue(velocity); d = motor_ctrl.compute(); motor = 1.0 - d; wait(interval); - pc.printf("BG Luminosity: %f, Feature Luminosity: %d, Contrast: %f, Exposure: %d\r\n", lum_bg, max, contrast, t_int); + pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); } } \ No newline at end of file