Diff: CAN/function_CAN.cpp

Revision:

252:179a8c8791dc

Parent:

235:4be664734d36

Child:

253:0246366e0dcb

--- a/CAN/function_CAN.cpp	Fri Jan 08 12:39:16 2021 +0000
+++ b/CAN/function_CAN.cpp	Tue Feb 16 07:12:23 2021 +0000
@@ -154,9 +154,9 @@
                     }
                 } 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 * 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) (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));
+                        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));
                     }
                 }
             }
@@ -166,21 +166,30 @@
 
 
             if (flag_data_request[2] == HIGH) {
+                
+                
                 double t_value = 0.0f;
-                if(value>=(float) VALVE_CENTER) {
-                    t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                double t_value_ref = 0.0f;
+                
+                if (CURRENT_CONTROL_MODE) {
+                    t_value = cur.sen;
+                    t_value_ref = I_REF_fil;
                 } else {
-                    t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-                }
-                double t_value_ref = 0.0f;
-                if(valve_pos.ref>=(float) VALVE_CENTER) {
-                    t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-                } else {
-                    t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                    if(value>=(float) VALVE_CENTER) {
+                        t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                    } else {
+                        t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                    }
+                    
+                    if(valve_pos.ref>=(float) VALVE_CENTER) {
+                        t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                    } else {
+                        t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                    }
                 }
 
-
-                CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+//                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
             }
 
 //            //If it doesn't rest, below can can not work.
@@ -372,8 +381,8 @@
 
 //            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
-            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
+            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
+            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
 
             break;
         }
@@ -722,7 +731,7 @@
             break;
         }
         case CRX_SET_VALVE_CENTER_OFFSET: {
-            VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
+            VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)));
             VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
 //            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
@@ -730,18 +739,18 @@
             break;
         }
         case CRX_SET_VALVE_DZ_MINUS_OFFSET: {
-            VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
+            VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)));
             VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET;
 //            ROM_RESET_DATA();
-            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
+            spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
 
             break;
         }
         case CRX_SET_VALVE_DZ_PLUS_OFFSET: {
-            VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
+            VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)));
             VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET;
 //            ROM_RESET_DATA();
-            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
+            spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
             break;
         }
         case CRX_SET_PID_GAIN_OPP: {