![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
asdgas
Dependencies: mbed Eigen FastPWM
Revision 65:549e0426cd91, committed 2020-05-18
- Comitter:
- Lightvalve
- Date:
- Mon May 18 00:39:31 2020 +0000
- Parent:
- 64:368bcd2a230a
- Commit message:
- 200518
Changed in this revision
CAN/function_CAN.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 368bcd2a230a -r 549e0426cd91 CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Tue May 12 06:24:31 2020 +0000 +++ b/CAN/function_CAN.cpp Mon May 18 00:39:31 2020 +0000 @@ -720,7 +720,7 @@ vel.ref = (double)temp_vel * 256.0f; } - torq.ref = (double)temp_torq * 0.1f; + torq.ref = (double)temp_torq * 0.1f / (float)(TORQUE_SENSOR_PULSE_PER_TORQUE); } else if(address==CID_RX_REF_VALVE_POS) { int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
diff -r 368bcd2a230a -r 549e0426cd91 main.cpp --- a/main.cpp Tue May 12 06:24:31 2020 +0000 +++ b/main.cpp Mon May 18 00:39:31 2020 +0000 @@ -1,3 +1,4 @@ +//200518 #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" @@ -693,10 +694,10 @@ double temp_vel_torq = 0.0f; double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control - if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s // L when P-gain = 100, f_cut = 10Hz L feedforward velocity - } else if ((OPERATING_MODE && 0x01) == 1) { + } else if ((OPERATING_MODE & 0x01) == 1) { temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s // L when P-gain = 100, f_cut = 10Hz L feedforward velocity } @@ -1311,7 +1312,7 @@ torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] // torque feedback - torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE) - torq.sen; //[N] + torq.err = torq_ref - torq.sen; //[N] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { @@ -1324,10 +1325,10 @@ double temp_vel_torq = 0.0f; double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control - if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s // L when P-gain = 100, f_cut = 10Hz L feedforward velocity - } else if ((OPERATING_MODE && 0x01) == 1) { + } else if ((OPERATING_MODE & 0x01) == 1) { temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s // L when P-gain = 100, f_cut = 10Hz L feedforward velocity } @@ -1335,12 +1336,12 @@ else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); // velocity compensation for torque control - if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s // L feedforward velocity - } else if ((OPERATING_MODE && 0x01) == 1) { + } else if ((OPERATING_MODE & 0x01) == 1) { I_REF_FORCE_FB = 0.001f * 0.01f*((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive. // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s @@ -1546,7 +1547,7 @@ } } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen)); + CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); } else if (SENSING_MODE == 1) { CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); }