Diff: callbacks.cpp

Revision:

56:85a26f839af2

Parent:

55:f102d271e808

--- a/callbacks.cpp	Thu May 21 02:19:25 2015 +0000
+++ b/callbacks.cpp	Sun Jan 31 06:44:58 2016 +0000
@@ -9,12 +9,18 @@
 
 void fast(Context *c) {
     float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
-    float I_b, I_c, angle, d_filtered, q_filtered, vd, vq;
+    float I_b, I_c, angle, d_filtered, q_filtered, vd, vq, speed;
     dbg_ib = I_b = c->motor->GetCurrentB();
     dbg_ic = I_c = c->motor->GetCurrentC();
     dbg_angle = angle = c->motor->GetPosition();
     float speed_raw = c->motor->GetSpeed();
-    dbg_speed = c->filter_sp->Update(speed_raw);
+    dbg_speed = speed = c->filter_sp->Update(speed_raw);
+    //float delta = speed / 5.0f;
+    float delta = 0.0f;
+    if (delta > 70.0f) delta = 70.0f;
+    angle += delta;
+    if (angle >= 360.0f) angle -= 360.0f;
+    if (angle < 0.0f) angle += 360.0f;
     
     Clarke(-(I_b + I_c), I_b, &alpha, &beta);
     Parke(alpha, beta, angle, &d, &q);
@@ -74,7 +80,7 @@
 
 void log(Context *c) {
     //c->debugger->Write(0, dbg_speed);
-    //c->debugger->Write(1, dbg_q_filtered);
-    //c->debugger->Write(2, dbg_d_filtered);
-    //c->debugger->Write(3, dbg_angle);
+    //c->debugger->Write(1, dbg_ib);
+    //c->debugger->Write(2, dbg_ic);
+    //c->debugger->Write(3, dbg_q_filtered);
 }
\ No newline at end of file