2011

Dependencies:   mbed FastPWM

Revision:
33:91b17819ec30
Parent:
32:4b8c0fedaf2c
Child:
34:bb2ca2fc2a8e
--- a/main.cpp	Mon Nov 11 09:48:00 2019 +0000
+++ b/main.cpp	Tue Nov 12 11:29:36 2019 +0000
@@ -238,28 +238,28 @@
     }
 }
 
-int DDV_JOINT_POS_FF(float REF_JOINT_VEL)
+float DDV_JOINT_POS_FF(float REF_JOINT_VEL)
 {
 
     int i = 0;
-    int Ref_Valve_Pos_FF = DDV_CENTER;
+    float Ref_Valve_Pos_FF = DDV_CENTER;
     for(i=0; i<VALVE_POS_NUM; i++) {
         if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <=  max(JOINT_VEL[i],JOINT_VEL[i+1])) {
             if(i==0) {
-                Ref_Valve_Pos_FF = ((int) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+                Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
             } else {
-                Ref_Valve_Pos_FF = ((int) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 10*ID_index_array[i-1];
+                Ref_Valve_Pos_FF = ((float) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + (float) (10*ID_index_array[i-1]);
             }
             break;
         }
     }
     if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
-        Ref_Valve_Pos_FF = VALVE_MAX_POS;
+        Ref_Valve_Pos_FF = (float) VALVE_MAX_POS;
     } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
-        Ref_Valve_Pos_FF = VALVE_MIN_POS;
+        Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
     }
     
-    Ref_Valve_Pos_FF = (int) ( (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + (float) DDV_CENTER);
+    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
     
     return Ref_Valve_Pos_FF;
 }
@@ -535,7 +535,7 @@
                 //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
                 VALVE_POS_RAW_FORCE_FB = 0.0f;
 
-                valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB  + VALVE_CENTER_OFFSET; // valve center offset = - 2.0f;
+                valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
                 //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
                 
                 if (valve_pos.ref > DDV_CENTER)
@@ -1402,17 +1402,13 @@
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
                                 SECOND_DZ = valve_pos.ref;
-                                DDV_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
+                                DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
                                 first_check = 0;
-                                VALVE_DEADZONE_MINUS = FIRST_DZ;
-                                VALVE_DEADZONE_PLUS = SECOND_DZ;
+                                VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
+                                VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
 
                                 ROM_RESET_DATA();
-
-                                //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
-                                //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
-                                //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
-                                //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
+                
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
@@ -1501,17 +1497,11 @@
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
                                 SECOND_DZ = valve_pos.ref;
-                                DDV_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
+                                DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
                                 first_check = 0;
-                                VALVE_DEADZONE_MINUS = FIRST_DZ;
-                                //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
-                                VALVE_DEADZONE_PLUS = SECOND_DZ;
+                                VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
+                                VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
                                 
-                                //ROM_RESET_DATA();
-
-                                //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
-                                //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
-                                //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
@@ -1551,12 +1541,12 @@
                         V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                         data_num = 0;
-                        valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
+                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
 
                         VALVE_POS_CONTROL(valve_pos.ref);
                         START_POS = pos.sen;
                     } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
+                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
                         VALVE_POS_CONTROL(valve_pos.ref);
                         data_num = data_num + 1;
                         if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
@@ -1717,9 +1707,7 @@
         if (flag_data_request[4] == HIGH) {
             //valve position
             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));
-
+            //CAN_TX_VALVE_POSITION((int16_t) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
 
             //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
             //                        CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));