![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 239:a9507d8a4674
- Parent:
- 238:bbb74caca589
- Child:
- 240:0757096870cb
--- 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;