Diff: main.cpp

Revision:

52:fd3d8df99287

Parent:

51:8b817927d6e4

Child:

53:b7f824360fe3

--- a/main.cpp	Sun Jan 22 05:23:01 2017 +0000
+++ b/main.cpp	Wed Jan 25 02:08:10 2017 +0000
@@ -48,9 +48,11 @@
 
 void commutate() {
     /*safety checks*/
-    if (control_enabled && !io.throttle_in->get_enabled()) go_disabled();
-    if (control_enabled && !io.pos->IsValid()) go_disabled();
-    if (!control_enabled && io.throttle_in->get_enabled()) go_enabled();
+    if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
+        go_disabled();
+    } else if (!control_enabled) {
+        go_enabled();
+    }
     
     /*update velocity, references*/
     update_velocity();
@@ -74,6 +76,9 @@
     park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
     
     /*PI controller*/
+    control.d_integral *= 1.0f - INTEGRAL_DECAY;
+    control.q_integral *= 1.0f - INTEGRAL_DECAY;
+    
     control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
     control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
         
@@ -130,10 +135,16 @@
 }
 
 void go_disabled() {
+    control.d_integral = 0.0f;
+    control.q_integral = 0.0f;
     control_enabled = false;
     io.en->write(0);
 }
 
+bool is_driving() {
+    return io.throttle_in->get_throttle() > 0.01f || read.w > 1.0f;
+}
+
 float update_filter(float old, float x, float str) {
     return str * old + (1.0f - str) * x;
 }
@@ -156,5 +167,7 @@
     BREMSInit(&io, &read, &foc, &control, false);
     
     for (;;) {
+        io.pc->printf("%f %f || %f %f || %f %d\n\r", control.d_integral, control.d_filtered, control.q_integral, control.q_filtered, read.w, is_driving());
+        wait(0.2);
     }
 }
\ No newline at end of file