![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- Revision:
- 261:10f0e04fdb0b
- Parent:
- 260:bbb74caca589
--- 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;