Diff: main.cpp

Revision:

120:57b6f3b1356b

Parent:

119:ad7a6af6fba3

Child:

121:de10418bf2c2

--- a/main.cpp	Tue Apr 25 04:49:46 2017 +0000
+++ b/main.cpp	Tue Apr 25 07:03:08 2017 +0000
@@ -49,6 +49,21 @@
     read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
 }
 
+void update_velocity2() {
+    read.last_p_mech2 = read.p_mech2;
+    read.p_mech2 = io.pos->GetUnlimitedMechPosition();
+    
+    float dp_mech2 = read.p_mech2 - read.last_p_mech2;
+    if (dp_mech2 < -PI) dp_mech2 += 2 * PI;
+    if (dp_mech2 >  PI) dp_mech2 -= 2 * PI;
+    
+    float w_raw2 = dp_mech2 * F_SW; //rad/s
+    if (w_raw2 > W_CRAZY) w_raw2 = read.w2; //with this limiting scheme noise < 0
+    if (w_raw2 < -W_CRAZY) w_raw2 = read.w2; //so we need to throw out the large deltas first
+    
+    read.w2 = update_filter(read.w2, w_raw2, W_FILTER_STRENGTH);
+}
+
 void commutate() {  
     /*safety checks, do we do anything this cycle?*/
     if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
@@ -57,6 +72,7 @@
     
     /*update velocity, references*/
     update_velocity();
+    update_velocity2();
     if (loop_counter % SLOW_LOOP_COUNTER == 0) {
         loop_counter = 0;
         slow_loop();
@@ -152,9 +168,10 @@
 }
 
 void log() {
-    io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), 
-                        (int) (255 * foc.vd), (int) (255 * foc.vq));
-    //if (io.pos->IsValid()) io.pc->printf("%f\n", io.pos->GetUnlimitedElecPosition());
+    //io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), 
+    //                    (int) (255 * foc.vd), (int) (255 * foc.vq));
+    //io.pc->printf("%d\n", io.pos->IsValid());
+    io.pc->printf("%f,%f\n", read.w, read.w2);
     wait(1.0f / LOG_FREQUENCY);
 }