Revision 2:a8adff46eaca, committed 2016-03-29

Comitter:

vsutardja

Date:

Tue Mar 29 23:31:32 2016 +0000

Parent:

1:32610c005260

Child:

3:c57674c348bd

Commit message:

Velocity moving weighted average

Changed in this revision

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