Diff: main.cpp

Revision:

65:549e0426cd91

Parent:

64:368bcd2a230a

--- 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));
                     }