Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
Diff: main.cpp
- Revision:
- 8:71c39d2f80e2
- Parent:
- 7:8229baaf1fbf
- Child:
- 9:10fcf3d0ec2d
diff -r 8229baaf1fbf -r 71c39d2f80e2 main.cpp --- a/main.cpp Fri Apr 01 20:41:10 2016 +0000 +++ b/main.cpp Fri Apr 01 21:41:41 2016 +0000 @@ -17,7 +17,8 @@ Timer t; telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0); telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); -telemetry:: +telemetry::Numeric<float> tele_vel(telemetry_obj, "t_vel", "Velocity", "m/s", 0); +telemetry::Numeric<uint32_t> tele_center(telemetry_obj, "t_center", "Center", "", 0); // ============= @@ -54,9 +55,9 @@ // ======== // Velocity // ======== -float Kp = 6.0; // Proportional factor -float Ki = 0; // Integral factor -float Kd = 0; // Derivative factor +float Kp = 3.0; // Proportional factor +float Ki = 1; // Integral factor +float Kd = 0.0001; // Derivative factor float interval = 0.01; // Sampling interval float ref_v = 1.0; PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller @@ -261,6 +262,7 @@ if (max > 43253) { center = (argmax + argmin + 2 + 11) / 2; + tele_center = center; a = 88 + (64 - center) * Ks; servo = a / 180; } @@ -282,7 +284,6 @@ servo.calibrate(0.001, 45.0); servo = a / 180.0; - wait(5); telemetry_serial.baud(115200); telemetry_serial.printf("t_h\r\n"); telemetry_obj.transmit_header(); @@ -308,18 +309,19 @@ // Start motor controller while (true) { // telemetry_serial.printf("%d\r\n", t.read_ms()); - if (dec == 10) { +// if (dec == 100) { tele_time_ms = t.read_ms(); telemetry_obj.do_io(); - dec = 0; - } - dec = dec + 1; +// dec = 0; +// } +// dec = dec + 1; 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 / 2.5; + tele_vel = velocity; //vel = velocity; // for (int i = 0; i < MVG_AVG; i++) { // velocity = velocity + v_prev[i];