Diff: main.cpp

Revision:

27:a2254a485f23

Parent:

26:8317d883d4f2

Child:

28:2a62d73e3dd0

--- a/main.cpp	Fri Sep 27 06:57:32 2019 +0000
+++ b/main.cpp	Tue Oct 01 05:18:16 2019 +0000
@@ -139,6 +139,7 @@
     MODE_DDV_POS_VS_PWM_ID = 30,                           //30
     MODE_DDV_DEADZONE_AND_CENTER,                       //31
     MODE_DDV_POS_VS_FLOWRATE,                           //32
+    MODE_ROM_RESET,                                     //33
 };
 
 int main()
@@ -321,6 +322,7 @@
     }
 
     V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+    //V_out = VALVE_PWM_RAW_FB;
 }
 
 #define LT_MAX_IDX  57
@@ -1219,7 +1221,7 @@
                             VALVE_POS_AVG_OLD = VALVE_MIN_POS;
                         }
                     }
-                    ROM_RESET_DATA();
+                    //ROM_RESET_DATA();
                     ID_index = 0;
                     CONTROL_MODE = MODE_NO_ACT;
                 }
@@ -1499,7 +1501,8 @@
                                 VALVE_DEADZONE_MINUS = FIRST_DZ;
                                 //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
                                 VALVE_DEADZONE_PLUS = SECOND_DZ;
-                                ROM_RESET_DATA();
+                                
+                                //ROM_RESET_DATA();
 
                                 //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
                                 //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
@@ -1583,7 +1586,7 @@
 
                         //spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
                         VALVE_POS_NUM = ID_index;
-                        ROM_RESET_DATA();
+                        //ROM_RESET_DATA();
                         ID_index = 0;
                         first_check = 0;
                         VALVE_FR_timer = 0;
@@ -1593,6 +1596,13 @@
                 }
                 break;
             }
+            case MODE_ROM_RESET: {
+                CAN_TX_TORQUE((int16_t) (7));
+                ROM_RESET_DATA();
+                
+                CONTROL_MODE = MODE_NO_ACT;
+                break;
+            }
 
             default:
                 break;
@@ -1627,6 +1637,12 @@
         /*******************************************************
         ***     PWM
         ********************************************************/
+        if (V_out >= 12000.0){
+            V_out = 12000.0;
+        }
+        else if(V_out<=-12000.0){
+            V_out = -12000.0;
+        }
         PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV
 
         // Saturation of output voltage to 5.0V
@@ -1702,7 +1718,7 @@
 
         if (flag_data_request[4] == HIGH) {
             //valve position
-            CAN_TX_VALVE_POSITION((int16_t) (value));
+            CAN_TX_VALVE_POSITION((int16_t) (value), (int16_t) valve_pos.ref, (int16_t) V_out);
             //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
             //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));