rainbow

Dependencies:   mbed FastPWM

Revision:
136:7e66fc71af40
Parent:
135:79885a39c161
Child:
137:ccf70b9b1705
--- a/main.cpp	Tue Oct 06 11:17:12 2020 +0000
+++ b/main.cpp	Tue Oct 06 11:29:47 2020 +0000
@@ -1616,6 +1616,11 @@
                 
                 V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*torq.err)/g4;
                 
+                float rho_gamma = 1.0f;
+                float gamma_hat_dot = rho3*torq.err/rho_gamma*((-f3+torq_ref_dot-k3*torq.err)/gamma_hat + g3_prime*(x_v-x_4_des));
+                gamma_hat = gamma_hat + gamma_hat_dot * (float) TMR_FREQ_5k;
+                
+                
                 /*       
                 double torq_ref = 0.0f;
                 pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]