Diff: main.cpp

Revision:

54:4c9415402628

Parent:

53:32218a36df05

Child:

55:489f01927025

--- a/main.cpp	Tue Sep 15 08:59:03 2020 +0000
+++ b/main.cpp	Thu Sep 17 07:49:27 2020 +0000
@@ -585,7 +585,8 @@
     reset_foc(&controller);                                                     // Reset current controller
     TIM1->CR1 ^= TIM_CR1_UDIS;
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
-    
+    reset_foc(&controller);                                                     // Reset current controller
+    reset_observer(&observer);                                                 // Reset observer
     wait(.1);
     NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0);                                             // commutation > communication
     
@@ -603,6 +604,8 @@
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
     
+                                                       // Set potision sensor nonlinearity lookup table
+    init_controller_params(&controller);
     //pc.baud(460800);//pc.baud(921600); 
     pc.baud(115000);
     //=====benk 2020 last version===//
@@ -716,7 +719,9 @@
             //printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position);
            if(count >= 4000){
 //            printf("J: %.3f  Mec: %.3f   Jerr: %.3f   JVerr: %.3f   Kp: %.3f   Kd: %.3f  \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd);
-            printf("Jraw:%.3f   J: %.3f  Mec: %.3f   N: %.3d   Nmod: %.3f   Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
+              //printf("Jraw:%.3f   J: %.3f  Mec: %.3f   N: %.3d   Nmod: %.3f   Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
+
+              printf("vd:%.3f   vq: %.3f   iq: %.3f  Velc:%.3f\n\r",controller.v_d,controller.v_q,controller.i_q_ref,controller.dtheta_elec);
 
               printf("Pdes: %.3f  Vdes: %.3f   Kp: %.3f   Kd: %.3f   Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff);
               printf("Prel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT);