One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of Everything by
Diff: main.cpp
- Revision:
- 2:a8adff46eaca
- Parent:
- 1:32610c005260
- Child:
- 3:c57674c348bd
--- a/main.cpp Tue Mar 29 22:00:52 2016 +0000 +++ b/main.cpp Tue Mar 29 23:31:32 2016 +0000 @@ -36,12 +36,14 @@ // ======= // Encoder // ======= +const int MVG_AVG = 4; int ppr = 389; // Pulses per revolution QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder float c = 0.20106; // Wheel circumference int prev_pulses = 0; // Previous pulse count int curr_pulses = 0; // Current pulse count float velocity = 0; // Velocity +float v_prev[MVG_AVG] = {0}; // ======== // Velocity @@ -92,6 +94,8 @@ bt.printf("Available diagnostics:\r\n"); 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"); cmd = bt.getc(); while (cmd != 'q') { switch(atoi(&cmd)){ @@ -101,6 +105,30 @@ 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("\r\n"); + Kp = atof(in); + 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("\r\n"); + t_int = atoi(in); + break; } if (bt.readable()) { cmd = bt.getc(); @@ -180,10 +208,9 @@ // Main // ==== int main() { + // Initialize motor motor.period_us(T); - motor = 1.0; - wait(7); motor = 1.0 - d; // Initialize servo @@ -197,7 +224,7 @@ // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); motor_ctrl.setOutputLimits(0.1, 0.5); - motor_ctrl.setSetPoint(3); + motor_ctrl.setSetPoint(1.5); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); @@ -208,16 +235,22 @@ Thread communication_thread(communication); // Start motor controller -// motor = 0.85; - - 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); prev_pulses = curr_pulses; motor_ctrl.setProcessValue(velocity); d = motor_ctrl.compute(); motor = 1.0 - d; wait(interval); + pc.printf("Velocity: %f\r\n", velocity); } } \ No newline at end of file