Diff: main.cpp

Revision:

40:3f2c0619c8c4

Parent:

38:118df027d851

Child:

41:abbd4e2af68b

--- a/main.cpp	Thu Nov 28 09:18:03 2019 +0000
+++ b/main.cpp	Thu Dec 19 11:39:31 2019 +0000
@@ -512,8 +512,6 @@
             }
 
             case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
-
-
                 float VALVE_POS_RAW_POS_FB = 0.0f; // Valve Position by Position Feedback
                 //float VALVE_POS_RAW_POS_FF = 0.0f; // Valve Position by Position Feedforward
                 float VALVE_POS_RAW_FORCE_FB = 0.0f; // Valve Position by Force Feedback
@@ -948,6 +946,7 @@
                     pos.ref_home_pos = pos.sen;
                     vel.ref_home_pos = 0.0f;
                     FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
+                    CAN_TX_PRES((int16_t)(CONTROL_MODE), (int16_t) (3));
                 } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
                     int cnt_check_enc = (TMR_FREQ_5k/500);
                     if(cnt_findhome%cnt_check_enc == 0) {
@@ -997,9 +996,10 @@
                     int T_move = 2*TMR_FREQ_5k;
                     pos.ref_home_pos = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
                     vel.ref_home_pos = 0.0f;
+                    CAN_TX_PRES((int16_t)(pos.ref_home_pos), (int16_t) (6));
 
                     // input for position control
-                    pos.err = (pos.ref_home_pos - (float)pos.sen) / ENC_PULSE_PER_POSITION;
+                    pos.err = pos.ref_home_pos - (float)pos.sen;
                     float VALVE_POS_RAW_POS_FB = 0.0f;
                     VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err;
                     VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;