Sungwoo Kim
/
HydraulicControlBoard_LIGHT
For LIGHT
Diff: function_utilities/function_utilities.cpp
- Revision:
- 30:8d561f16383b
- Parent:
- 29:69f3f5445d6d
- Child:
- 32:4b8c0fedaf2c
diff -r 69f3f5445d6d -r 8d561f16383b function_utilities/function_utilities.cpp --- 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 @@ } +