Diff: CAN/function_CAN.cpp

Revision:

256:7c8cc8b56b88

Parent:

254:725d7435dfa3

--- a/CAN/function_CAN.cpp	Mon Feb 22 14:09:13 2021 +0000
+++ b/CAN/function_CAN.cpp	Wed Mar 03 12:23:13 2021 +0000
@@ -137,12 +137,6 @@
         case CRX_SET_DATA_REQUEST: {
             int request_type = msg.data[2];
             flag_data_request[request_type] = msg.data[1];
-            //pc.printf("can middle %d\n", request_type);
-
-//            if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
-            //if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3;
-//            if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
-            //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
             
             // Position, Velocity, and Torque (ID:1200)
             if (flag_data_request[0] == HIGH) {
@@ -154,8 +148,7 @@
                     }
                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                     if (SENSING_MODE == 0) {
-//                        CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
-                        CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (pos.ref*0.001f));
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/16.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/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
                     }
@@ -173,8 +166,8 @@
                 double t_value_ref = 0.0f;
                 
                 if (CURRENT_CONTROL_MODE) {
-                    t_value = cur.sen;
-                    t_value_ref = I_REF_fil;
+                    t_value = cur.sen * 1000.0f;    //Pulse
+                    t_value_ref = I_REF_fil * 1000.0f;  //Pulse
                 } else {
                     if(value>=(float) VALVE_CENTER) {
                         t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
@@ -189,8 +182,7 @@
                     }
                 }
 
-//                CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
-                CAN_TX_PRES((int16_t) (VALVE_DEADZONE_PLUS), (int16_t) (t_value_ref)); // 1400
+                CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
             }
 
 //            //If it doesn't rest, below can can not work.
@@ -851,7 +843,7 @@
             pos.ref = (double)temp_pos * 1.0f;
             vel.ref = (double)temp_vel * 10.0f;
         } else { //Linear Actuator
-            pos.ref = (double)temp_pos * 10.0f;
+            pos.ref = (double)temp_pos * 16.0f;
             vel.ref = (double)temp_vel * 256.0f;
         }