Diff: main.cpp

Revision:

121:de10418bf2c2

Parent:

120:57b6f3b1356b

Child:

122:53be0630f79d

--- a/main.cpp	Tue Apr 25 07:03:08 2017 +0000
+++ b/main.cpp	Tue Apr 25 07:21:42 2017 +0000
@@ -43,27 +43,12 @@
     if (dp_mech >  PI) dp_mech -= 2 * PI;
     
     float w_raw = dp_mech * F_SW; //rad/s
-    if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0
-    if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
+    if (w_raw > W_CRAZY) w_raw = read.w; //throw out inplausible results
+    if (w_raw < -W_CRAZY) w_raw = read.w; 
     
     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()) {
@@ -72,7 +57,6 @@
     
     /*update velocity, references*/
     update_velocity();
-    update_velocity2();
     if (loop_counter % SLOW_LOOP_COUNTER == 0) {
         loop_counter = 0;
         slow_loop();
@@ -168,10 +152,9 @@
 }
 
 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));
+    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);
 }
         
@@ -200,13 +183,6 @@
     th = new NullThrottleMapper();
     BREMSInit(&io, &read, &foc, &control, false);
     
-    /*
-    for (;;) {
-        float x = io.throttle_in->get_throttle();
-        if (x > 0.5f) calibrate_position(&io);
-    }
-    */ 
-    
     for (;;) {
         if (ENABLE_LOGGING) {
             log();