zebra joint controller firmware (originally from MIT HKC motor controller by benkatz)

Dependencies:   mbed FastPWM3

Revision:
55:1c704b515383
Parent:
53:e85efce8c1eb
--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Fri Feb 05 10:49:02 2021 +0000
@@ -147,6 +147,8 @@
            }
        controller->i_a = -controller->i_b - controller->i_c;       
        
+       //printf("%f %f %f\r\n",controller->i_a, controller->i_b, controller->i_c);
+       
        float s = FastSin(theta); 
        float c = FastCos(theta);                            
        dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
@@ -155,7 +157,7 @@
         
         controller->i_q_filt = 0.95f*controller->i_q_filt + 0.05f*controller->i_q;
         controller->i_d_filt = 0.95f*controller->i_d_filt + 0.05f*controller->i_d;
-        
+        //printf("%f, %f, %f, %f, %f\r\n",controller->i_a, controller->i_b, controller->i_c, controller->i_d_filt, controller->i_q_filt);
         
         // Filter the current references to the desired closed-loop bandwidth
         controller->i_d_ref_filt = (1.0f-controller->alpha)*controller->i_d_ref_filt + controller->alpha*controller->i_d_ref;
@@ -224,5 +226,6 @@
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
+    //printf("pos:%f  vel:%f  Kp:%f  Kd:%f  t_ff:%f\r\n",controller->p_des,controller->v_des,controller->kp,controller->kd,controller->t_ff);
     }
  
\ No newline at end of file