Diff: main.cpp

Revision:

69:3995ffeaa786

Parent:

68:328e1be06f5d

Child:

70:d049d9b7dc6b

--- a/main.cpp	Tue May 12 11:25:00 2020 +0000
+++ b/main.cpp	Fri May 15 08:38:26 2020 +0000
@@ -305,7 +305,7 @@
     /*********************************
     ***     Initialization
     *********************************/
-    LED = 1;
+    LED = 0;
     pc.baud(9600);
 
     // i2c init
@@ -453,12 +453,15 @@
             } else {
                 valve_pos.ref = -output*0.0001f*((double)VALVE_MIN_POS - (double) VALVE_CENTER) + (double) VALVE_CENTER;
             }
+            
+            if(LED==1) {
+                LED=0;
+            } else
+                LED = 1;
+            
         }
         
-        if(LED==1) {
-             LED=0;
-        } else
-            LED = 1;
+        
 
     }
 }
@@ -920,10 +923,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
                         }
@@ -1551,10 +1554,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
                     }
@@ -1562,12 +1565,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