Diff: main.cpp

Revision:

261:10f0e04fdb0b

Parent:

260:bbb74caca589

diff -r bbb74caca589 -r 10f0e04fdb0b main.cpp
--- a/main.cpp	Sat Jul 10 04:39:20 2021 +0000
+++ b/main.cpp	Thu Oct 06 06:55:07 2022 +0000
@@ -2,7 +2,7 @@
 //distributed by Sungwoo Kim
 //       2020/12/28
 //revised by Buyoun Cho
-//       2021/04/20
+//       2022/10/06
 
 // 유의사항
 // 소수 적을때 뒤에 f 꼭 붙이기
@@ -94,6 +94,7 @@
 
 float temp_P_GAIN = 0.0f;
 float temp_I_GAIN = 0.0f;
+float temp_LOG[4] = {0.0f,};
 int temp_VELOCITY_COMP_GAIN = 0;
 int logging = 0;
 
@@ -240,8 +241,8 @@
     make_delay();
 
     ////// bno rom
-//    spi_eeprom_write(RID_BNO, (int16_t) 2);
-//    make_delay();
+    spi_eeprom_write(RID_BNO, (int16_t) 3);
+    make_delay();
     ////////
 
     // rom
@@ -589,7 +590,7 @@
                             float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
                             FORCE_pulse_sum = 0.0f;
 
-                            FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
+                            FORCE_VREF -= VREF_TuningGain * (0.0f - FORCE_pluse_mean);
                             if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
                             if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
                             dac_1 = FORCE_VREF / 3.3f;
@@ -630,6 +631,9 @@
                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
                     TMR3_COUNT_TORQUE_NULL = 0;
                 }
+//                dac_1 = 1.65f / 3.3f;
+//                CONTROL_UTILITY_MODE = MODE_NO_ACT;
+
                 break;
             }
 
@@ -763,12 +767,12 @@
                 D_LPF = (1.0f-alpha_SpringDamper) * D_LPF + alpha_SpringDamper * D_DAMPER;
 
                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
-                    float torq_ref_act = torq.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : Nm
+                    float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm
                     torq.err = torq_ref_act - torq.sen;
                     torq.err_int += torq.err/((float)TMR_FREQ_5k);
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
                 } else {
-                    float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N
+                    float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N
                     force_ref_act_can = force_ref_act;
                     force.err = force_ref_act - force.sen;
                     force.err_int += force.err/((float)TMR_FREQ_5k);
@@ -786,6 +790,9 @@
                 // command integration =================================================================================================================================================
                 temp_vel = (1.0f - alpha_trans) * temp_vel_pos  + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward
                 
+                temp_LOG[0] = temp_vel_FT;
+                temp_LOG[1] = temp_vel_ff;
+
                 float Qact = 0.0f; // required flow rate
                 if( temp_vel > 0.0f ) {
                     Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM
@@ -794,9 +801,9 @@
                     Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM
                     I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
                 }
-                
+
                 float I_MAX = 10.0f; // Maximum Current : 10mA
-                
+
                 // Anti-windup for FT
 //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
                 if (I_GAIN_JOINT_TORQUE > 0.001f) {
@@ -816,7 +823,7 @@
                     if(I_REF > I_MAX) {
                         I_REF = I_MAX;
                     } else if (I_REF < -I_MAX) {
-                        I_REF = -I_MAX;  
+                        I_REF = -I_MAX;
                     }
                 }
                 break;
@@ -925,7 +932,7 @@
                 // Moog Valve Current Control Gain
                 double R_model = 500.0f; // ohm
                 double L_model = 1.2f;
-                double w0 = 2.0f * 3.14f * 150.0f;
+                double w0 = 2.0f * 3.14f * 50.0f;
                 double KP_I = 0.1f * L_model*w0;
                 double KI_I = 0.1f * R_model*w0;
 
@@ -1019,7 +1026,8 @@
         //////////////////////  Data transmission through CAN //////////////////////
         ////////////////////////////////////////////////////////////////////////////
 
-        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+//        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+        if (TMR2_COUNT_CAN_TX % 12 == 0) {
 
             // Position, Velocity, and Torque (ID:1200)
             if (flag_data_request[0] == HIGH) {
@@ -1041,7 +1049,8 @@
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
             if (flag_data_request[2] == HIGH) {
-                CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (force_ref_act_can*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (temp_LOG[0]), (int16_t) (temp_LOG[1]));
+//                CAN_TX_SOMETHING((int16_t)(temp_LOG[0]), (int16_t)(temp_LOG[1]), (int16_t) (temp_LOG[0]), (int16_t) (temp_LOG[1]));
             }
 
             TMR2_COUNT_CAN_TX = 0;