Diff: main.cpp

Revision:

8:71c39d2f80e2

Parent:

7:8229baaf1fbf

Child:

9:10fcf3d0ec2d

--- 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];