Revision 262:a9507d8a4674, committed 2022-10-06

Comitter:

Lightvalve

Date:

Thu Oct 06 07:18:55 2022 +0000

Parent:

260:bbb74caca589

Commit message:

Ready to Move to Buyoun's Keil Studio (Pressure_f_cut_20)

Changed in this revision

--- a/CAN/function_CAN.cpp	Sat Jul 10 04:39:20 2021 +0000
+++ b/CAN/function_CAN.cpp	Thu Oct 06 07:18:55 2022 +0000
@@ -506,8 +506,8 @@
         }
 
         case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
-            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f);
-            spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*100.0f));
+            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.001f);
+            spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*1000.0f));
 
             break;
         }
@@ -1107,7 +1107,7 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
-    int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f);
+    int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 1000.0f);
     temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
     temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
 

--- a/function_utilities/function_utilities.cpp	Sat Jul 10 04:39:20 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Thu Oct 06 07:18:55 2022 +0000
@@ -437,7 +437,7 @@
     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
     STROKE = spi_eeprom_read(RID_STROKE);
     ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
-    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.01f;
+    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.001f;
     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
 //    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;

--- a/main.cpp	Sat Jul 10 04:39:20 2021 +0000
+++ b/main.cpp	Thu Oct 06 07:18:55 2022 +0000
@@ -240,8 +240,8 @@
     make_delay();
 
     ////// bno rom
-//    spi_eeprom_write(RID_BNO, (int16_t) 2);
-//    make_delay();
+//    spi_eeprom_write(RID_BNO, (int16_t) 9);
+    make_delay();
     ////////
 
     // rom
@@ -469,10 +469,10 @@
 
             if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
                 float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
-                torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f);  // unit : Nm
+                torq.UpdateSen(torq_new,FREQ_TMR4,20.0f);  // unit : Nm
             } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                 float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
-                force.UpdateSen(force_new,FREQ_TMR4,1000.0f);  // unit : N
+                force.UpdateSen(force_new,FREQ_TMR4,20.0f);  // unit : N
             }
         }
 
@@ -763,12 +763,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);
@@ -925,7 +925,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;
 
@@ -1041,7 +1041,7 @@
 
             // 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) (K_SPRING), (int16_t) (D_DAMPER));
             }
 
             TMR2_COUNT_CAN_TX = 0;