Diff: function_utilities/function_utilities.cpp

Revision:

30:8d561f16383b

Parent:

29:69f3f5445d6d

Child:

32:4b8c0fedaf2c

--- a/function_utilities/function_utilities.cpp	Wed Oct 02 04:41:59 2019 +0000
+++ b/function_utilities/function_utilities.cpp	Fri Oct 04 11:58:18 2019 +0000
@@ -40,18 +40,18 @@
 int16_t DIR_VALVE = 0;
 int16_t DIR_VALVE_ENC = 0;
 
-double SUPPLY_VOLTAGE = 12000.0;
-double VALVE_VOLTAGE_LIMIT = 12000.0;  //mv
+float SUPPLY_VOLTAGE = 12000.0f;
+float VALVE_VOLTAGE_LIMIT = 12000.0f;  //mv
 
-double P_GAIN_VALVE_POSITION = 0.0;
-double I_GAIN_VALVE_POSITION= 0.0;
-double D_GAIN_VALVE_POSITION= 0.0;
-double P_GAIN_JOINT_POSITION = 0.0;
-double I_GAIN_JOINT_POSITION = 0.0;
-double D_GAIN_JOINT_POSITION = 0.0;
-double P_GAIN_JOINT_TORQUE = 0.0;
-double I_GAIN_JOINT_TORQUE = 0.0;
-double D_GAIN_JOINT_TORQUE = 0.0;
+float P_GAIN_VALVE_POSITION = 0.0f;
+float I_GAIN_VALVE_POSITION= 0.0f;
+float D_GAIN_VALVE_POSITION= 0.0f;
+float P_GAIN_JOINT_POSITION = 0.0f;
+float I_GAIN_JOINT_POSITION = 0.0f;
+float D_GAIN_JOINT_POSITION = 0.0f;
+float P_GAIN_JOINT_TORQUE = 0.0f;
+float I_GAIN_JOINT_TORQUE = 0.0;
+float D_GAIN_JOINT_TORQUE = 0.0;
 
 int16_t VALVE_DEADZONE_PLUS;
 int16_t VALVE_DEADZONE_MINUS;
@@ -70,7 +70,7 @@
 
 int16_t PISTON_AREA_A;
 int16_t PISTON_AREA_B;
-double PISTON_AREA_alpha;
+float PISTON_AREA_alpha;
 
 
 int16_t PRES_SUPPLY;
@@ -87,20 +87,20 @@
 
 int16_t ENC_PULSE_PER_POSITION;
 int16_t TORQUE_SENSOR_PULSE_PER_TORQUE;
-double PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
-double PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
+float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
+float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
 
 int HOMEPOS_OFFSET;
 int HOMEPOS_VALVE_OPENING;
 
-double FRICTION;
-double REF_PERIOD;
-double REF_MAG;
+float FRICTION;
+float REF_PERIOD;
+float REF_MAG;
 int REF_NUM;
 
 
-double DAC_REF;
-double DAC_RESOL;
+float DAC_REF;
+float DAC_RESOL;
 
 int REF_POSITION;
 int REF_VELOCITY;
@@ -121,9 +121,9 @@
 
 int CUR_POSITION;
 int CUR_VELOCITY;
-double CUR_TORQUE;
-double CUR_PRES_A;
-double CUR_PRES_B;
+float CUR_TORQUE;
+float CUR_PRES_A;
+float CUR_PRES_B;
 int CUR_PWM;
 int CUR_VALVE_POSITION;
 
@@ -144,24 +144,24 @@
 ////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////
 
-double CUR_PRES_A_BAR;
-double CUR_PRES_B_BAR;
-double CUR_TORQUE_NM;
-double CUR_TORQUE_NM_PRESS;
+float CUR_PRES_A_BAR;
+float CUR_PRES_B_BAR;
+float CUR_TORQUE_NM;
+float CUR_TORQUE_NM_PRESS;
 
-double PRES_A_VREF;
-double PRES_A_VREF_TEST;
-double PRES_B_VREF;
-double PRES_B_VREF_TEST;
-double TORQUE_VREF;
+float PRES_A_VREF;
+float PRES_A_VREF_TEST;
+float PRES_B_VREF;
+float PRES_B_VREF_TEST;
+float TORQUE_VREF;
 
-double VALVE_PWM_RAW_FB;
-double VALVE_PWM_RAW_FF;
+float VALVE_PWM_RAW_FB;
+float VALVE_PWM_RAW_FF;
 int VALVE_PWM_VALVE_DZ;
 int VALVE_INPUT_PWM;
 
-double VALVE_GAIN_LPM_PER_V[10];
-double VALVE_POS_VS_PWM[25];
+float VALVE_GAIN_LPM_PER_V[10];
+float VALVE_POS_VS_PWM[25];
 long JOINT_VEL[100];
 
 int VALVE_MAX_POS;
@@ -186,10 +186,10 @@
 bool FLAG_REFERENCE_PRES_DIFF;
 bool FLAG_REFERENCE_CURRENT;
 
-double TUNING_TIME;
+float TUNING_TIME;
 
-double REFERENCE_FREQ;
-double REFERENCE_MAG;
+float REFERENCE_FREQ;
+float REFERENCE_MAG;
 
 bool FLAG_FIND_HOME;
 
@@ -202,18 +202,18 @@
     JUMP_LANDING,                                  //4
 };
 
-double CUR_PRES_DIFF_BAR = 0.0;
-double CUR_PRES_A_sum = 0.0;
-double CUR_PRES_B_sum = 0.0;
-double CUR_PRES_A_mean = 0.0;
-double CUR_PRES_B_mean = 0.0;
-double CUR_TORQUE_sum = 0.0;
-double CUR_TORQUE_mean = 0.0;
-double PRES_A_NULL = 1.0;
-double PRES_B_NULL = 1.0;
-double TORQUE_NULL = 3900;
+float CUR_PRES_DIFF_BAR = 0.0f;
+float CUR_PRES_A_sum = 0.0f;
+float CUR_PRES_B_sum = 0.0f;
+float CUR_PRES_A_mean = 0.0f;
+float CUR_PRES_B_mean = 0.0f;
+float CUR_TORQUE_sum = 0.0f;
+float CUR_TORQUE_mean = 0.0f;
+float PRES_A_NULL = 1.0f;
+float PRES_B_NULL = 1.0f;
+float TORQUE_NULL = 3900.0f;
 
-double Ref_Valve_Pos_Old = 0.0;
+float Ref_Valve_Pos_Old = 0.0f;
 
 int VALVE_ID_timer = 0;
 int VALVE_DZ_timer = 0;
@@ -228,7 +228,7 @@
 int DZ_index = 1;
 int ID_index_array[50] = {0};
 int first_check = 0;
-double init_time = 0.0;
+float init_time = 0.0f;
 int VEL_POINT = 0;
 int DZ_case = 0;
 int START_POS = 0;
@@ -238,19 +238,19 @@
 int SECOND_DZ = 0;
 int DZ_NUM = 0;
 int one_period_end = 0;
-double Ref_Vel_Test = 0.0;
+float Ref_Vel_Test = 0.0f;
 long TMR2_FOR_SLOW_LOGGING = 0;
 //int velcount = 0;
 char max_check = 0;
 char min_check = 0; 
 
-double valve_pos_err = 0.0, valve_pos_err_old = 0.0, valve_pos_err_diff = 0.0, valve_pos_err_sum = 0.0;
-double joint_pos_err = 0.0, joint_pos_err_old = 0.0, joint_pos_err_diff = 0.0, joint_pos_err_diff_fil = 0.0, joint_pos_err_sum = 0.0;
-double joint_torq_err = 0.0, joint_torq_err_old = 0.0, joint_torq_err_diff = 0.0, joint_torq_err_sum = 0.0;
-double VALVE_PWM_RAW_POS = 0.0, VALVE_PWM_RAW_TORQ = 0.0;
+float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f;
+float joint_pos_err = 0.0f, joint_pos_err_old = 0.0f, joint_pos_err_diff = 0.0f, joint_pos_err_diff_fil = 0.0f, joint_pos_err_sum = 0.0f;
+float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
+float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
 
-double CUR_FLOWRATE = 0.0;
-double VALVE_FF_VOLTAGE = 0.0;
+float CUR_FLOWRATE = 0.0f;
+float VALVE_FF_VOLTAGE = 0.0f;
 
 int pos_plus_end = 0;
 int pos_minus_end = 0;
@@ -259,8 +259,8 @@
 
 int temp_time = 0;
 
-double CUR_VELOCITY_sum = 0.0;
-double temp_vel_sum = 0.0;
+float CUR_VELOCITY_sum = 0.0f;
+float temp_vel_sum = 0.0f;
 
 int DZ_dir = 0;
 int DZ_temp_cnt = 0;
@@ -285,22 +285,22 @@
 int check_vel_pos_fin = 0;
 int check_vel_pos_interv = 0;
 int valve_gain_repeat_cnt = 0;
-double VALVE_VOLTAGE = 0.0;
+float VALVE_VOLTAGE = 0.0f;
 
-double freq_fric_tune = 1.0;
+float freq_fric_tune = 1.0f;
 
 bool FLAG_VALVE_OUTPUT_CALIB = false;
 
 uint32_t TMR3_COUNT_CAN_TX = 0;
 
-double I_REF = 0.0;
+float I_REF = 0.0f;
 int TMR3_COUNT_IREF = 0;
-double CUR_CURRENT = 0.0;
-double u_CUR[3] = {0.0,0.0,0.0};
+float CUR_CURRENT = 0.0f;
+float u_CUR[3] = {0.0f,0.0f,0.0f};
 int FINDHOME_STAGE = 0;
 int FINDHOME_INIT = 0;
-int FINDHOME_GOTOLIMIT = 0;
-int FINDHOME_ZEROPOSE = 0;
+int FINDHOME_GOTOLIMIT = 1;
+int FINDHOME_ZEROPOSE = 2;
 
 //int h1, h2, h3, h4, h5, h6;
 
@@ -309,25 +309,25 @@
  ******************************************************************************/
 
 
-double dabs(double tx) {
-    if (tx >= 0.0)
+float dabs(float tx) {
+    if (tx >= 0.0f)
         return tx;
     else
         return -tx;
 }
 
-double change_int_to_efloat(int input) {
+float change_int_to_efloat(int input) {
     int i = 0;
 
-    double output = 0;
-    int vn = (int) ((double) input / 10.);
+    float output = 0;
+    int vn = (int) ((float) input / 10.);
     int en = input % 10;
 
-    double temp = 1.;
+    float temp = 1.;
     for (i = 0; i < en; i++)
-        temp *= 0.1;
+        temp *= 0.1f;
 
-    output = (double) vn*temp;
+    output = (float) vn*temp;
     return output;
 }
 
@@ -354,8 +354,8 @@
     writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC);
     writer.write(RID_VALVE_DIR,(int) DIR_VALVE);
     writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC);
-    writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0));
-    writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0));
+    writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0f));
+    writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0f));
     writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION);
     writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION);
     writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION);
@@ -385,24 +385,24 @@
     writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
     writer.write(RID_ENC_PULSE_PER_POSITION,(int) ENC_PULSE_PER_POSITION);
     writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) TORQUE_SENSOR_PULSE_PER_TORQUE);
-    writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
-    writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
-    writer.write(RID_FRICTION,(int) (FRICTION * 10.0)); 
+    writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
+    writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
+    writer.write(RID_FRICTION,(int) (FRICTION * 10.0f)); 
     writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET); 
     writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING); 
-    writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0)); 
-    writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0));
-    writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0));
-    writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0)); 
-    writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0));
+    writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f)); 
+    writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f));
+    writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f));
+    writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f)); 
+    writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
     for(int i=0; i<25; i++)
     {
         writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]);
@@ -423,16 +423,16 @@
 
 void ROM_CALL_DATA(void) {
     BNO = flashReadInt(Rom_Sector, RID_BNO);
-    BNO = 11;
+    BNO = 5;
     OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
     CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
     DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR);
     DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
     DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
-    SUPPLY_VOLTAGE = (double) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1;
-    SUPPLY_VOLTAGE = 12000.0;
-    VALVE_VOLTAGE_LIMIT = (double) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1;
-    VALVE_VOLTAGE_LIMIT = 12000.0;
+    SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
+    SUPPLY_VOLTAGE = 12000.0f;
+    VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
+    VALVE_VOLTAGE_LIMIT = 12000.0f;
     P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
     I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
     D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
@@ -453,7 +453,7 @@
     CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
     PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
     PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
-    PISTON_AREA_alpha = (double)PISTON_AREA_B/(double)PISTON_AREA_A;
+    PISTON_AREA_alpha = (float)PISTON_AREA_B/(float)PISTON_AREA_A;
     PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
     PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
     ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
@@ -463,29 +463,29 @@
     VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
     ENC_PULSE_PER_POSITION = flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION);
     TORQUE_SENSOR_PULSE_PER_TORQUE = flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
-    PRES_SENSOR_A_PULSE_PER_BAR = (double) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01;
-    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
-    PRES_SENSOR_B_PULSE_PER_BAR = (double) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01;
-    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
-    FRICTION = (double) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1;
+    PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
+    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
+    PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
+    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
+    FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
     HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
     HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
-    TORQUE_VREF = (double) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001;
-    PRES_A_VREF = (double) flashReadInt(6, RID_PRES_A_SENSOR_VREF) * 0.001;
-    PRES_B_VREF = (double) flashReadInt(6, RID_PRES_B_SENSOR_VREF) * 0.001;
-    VALVE_GAIN_LPM_PER_V[0] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[2] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[4] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[6] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[8] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[1] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[3] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[5] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[7] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01;
-    VALVE_GAIN_LPM_PER_V[9] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01;
+    TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
+    PRES_A_VREF = (float) flashReadInt(6, RID_PRES_A_SENSOR_VREF) * 0.001f;
+    PRES_B_VREF = (float) flashReadInt(6, RID_PRES_B_SENSOR_VREF) * 0.001f;
+    VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
     for(int i=0; i<25; i++)
     {
-        VALVE_POS_VS_PWM[i] = (double) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
+        VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
     }
     for(int i=0; i<100; i++)
     {
@@ -502,20 +502,20 @@
  * ENCODER functions
  ******************************************************************************/
 // A-KHA
-#define     KF_G1_11    0.083920206005350
-#define     KF_G1_12    0.000013905329560
-#define     KF_G1_21    -0.000575742328210
-#define     KF_G1_22    0.799999939711725
+#define     KF_G1_11    0.083920206005350f
+#define     KF_G1_12    0.000013905329560f
+#define     KF_G1_21    -0.000575742328210f
+#define     KF_G1_22    0.799999939711725f
 // K
-#define     KF_G2_11    0.916079793994650
-#define     KF_G2_12    0.000002878711641
-#define     KF_G2_21    0.000575742328210
-#define     KF_G2_22    0.199999945139809
+#define     KF_G2_11    0.916079793994650f
+#define     KF_G2_12    0.000002878711641f
+#define     KF_G2_21    0.000575742328210f
+#define     KF_G2_22    0.199999945139809f
 
-double KF_Y_11 = 0.;
-double KF_Y_21 = 0.;
-double KF_X_11 = 0.;
-double KF_X_21 = 0.;
+float KF_Y_11 = 0.0f;
+float KF_Y_21 = 0.0f;
+float KF_X_11 = 0.0f;
+float KF_X_21 = 0.0f;
 
 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
@@ -565,3 +565,4 @@
 }
 
 
+