Revision 41:abbd4e2af68b, committed 2019-12-23

Comitter:

Lightvalve

Date:

Mon Dec 23 04:05:31 2019 +0000

Parent:

40:3f2c0619c8c4

Child:

42:1cf66990ccab

Commit message:

191223

Changed in this revision

--- a/CAN/function_CAN.cpp	Thu Dec 19 11:39:31 2019 +0000
+++ b/CAN/function_CAN.cpp	Mon Dec 23 04:05:31 2019 +0000
@@ -715,10 +715,15 @@
         unsigned int CMD = msg.data[0];
         ReadCMD(CMD);
     } else if(address==CID_RX_REF_POSITION) {
-        int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
-        int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
-        pos.ref = (double)temp_pos;
-        vel.ref = (double)temp_vel;
+        //int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
+        int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8);
+        //int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
+        int32_t temp_vel = (int32_t) (msg.data[2] | msg.data[3] << 8);
+        
+        int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
+        pos.ref = (double)temp_pos * 4.0f;
+        vel.ref = (double)temp_vel * 400.0f;
+        torq.ref = (double)temp_torq;
     } else if(address==CID_RX_REF_TORQUE) {
         int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
         torq.ref = (double)temp_torq;
@@ -1265,19 +1270,17 @@
  Sensor & State Transmission Functions
 *******************************************************************************/
 
-void CAN_TX_POSITION(int32_t t_pos, int32_t t_vel) {
+void CAN_TX_POSITION(int16_t t_pos, int16_t t_vel, int16_t t_torq) {
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_POSITION;
     temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
-    temp_msg.data[2] = (uint8_t) (t_pos >> 16);
-    temp_msg.data[3] = (uint8_t) (t_pos >> 24);
-    temp_msg.data[4] = (uint8_t) t_vel;
-    temp_msg.data[5] = (uint8_t) (t_vel >> 8);
-    temp_msg.data[6] = (uint8_t) (t_vel >> 16);
-    temp_msg.data[7] = (uint8_t) (t_vel >> 24);
+    temp_msg.data[2] = (uint8_t) t_vel;
+    temp_msg.data[3] = (uint8_t) (t_vel >> 8);
+    temp_msg.data[4] = (uint8_t) t_torq;
+    temp_msg.data[5] = (uint8_t) (t_torq >> 8);
 
     can.write(temp_msg);
 }

--- a/CAN/function_CAN.h	Thu Dec 19 11:39:31 2019 +0000
+++ b/CAN/function_CAN.h	Mon Dec 23 04:05:31 2019 +0000
@@ -143,7 +143,7 @@
 #define             CTX_VALVE_POS_NUM                           45
 #define             CTX_VALVE_MAX_MIN_POS                       46
 // Sensor & State Transmission
-void CAN_TX_POSITION(int32_t t_pos, int32_t t_vel);
+void CAN_TX_POSITION(int16_t t_pos, int16_t t_vel, int16_t t_torq);
 void CAN_TX_TORQUE(int16_t t_torque);
 void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b);
 void CAN_TX_PWM(int16_t t_pwm);

--- a/main.cpp	Thu Dec 19 11:39:31 2019 +0000
+++ b/main.cpp	Mon Dec 23 04:05:31 2019 +0000
@@ -996,7 +996,6 @@
                     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;
@@ -1677,11 +1676,8 @@
         //CAN ----------------------------------------------------------------------
         if (flag_data_request[0] == HIGH) {
             //position+velocity
-            CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
-            //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
-            //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
-            //pc.printf("can good");
-            //                                    CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
+            //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/400.0f), (int16_t) torq.sen);
         }
 
         if (flag_data_request[1] == HIGH) {