Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

Dependencies:   mbed FastPWM

function_utilities/function_utilities.cpp@58:2eade98630e2, 2020-03-09 (annotated)

Committer:
Lightvalve
Date:
Mon Mar 09 12:43:04 2020 +0000
Revision:
58:2eade98630e2
Parent:
57:f4819de54e7a
Child:
59:0ad14153b58f
Integrated finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lightvalve 11:82d8768d7351 1 #include "setting.h"
Lightvalve 11:82d8768d7351 2 #include "SPI_EEP_ENC.h"
Lightvalve 11:82d8768d7351 3 #include "function_utilities.h"
Lightvalve 14:8e7590227d22 4 #include "function_CAN.h"
Lightvalve 16:903b5a4433b4 5 #include "stm32f4xx_flash.h"
Lightvalve 16:903b5a4433b4 6 #include "FlashWriter.h"
Lightvalve 16:903b5a4433b4 7
Lightvalve 16:903b5a4433b4 8 int Rom_Sector = 6;
Lightvalve 16:903b5a4433b4 9 //FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
Lightvalve 16:903b5a4433b4 10
Lightvalve 11:82d8768d7351 11 /*******************************************************************************
Lightvalve 11:82d8768d7351 12 * VARIABLE
Lightvalve 11:82d8768d7351 13 ******************************************************************************/
Lightvalve 11:82d8768d7351 14
Lightvalve 34:bb2ca2fc2a8e 15 // Board Information
Lightvalve 11:82d8768d7351 16 uint8_t BNO = 0;
Lightvalve 11:82d8768d7351 17 uint8_t CONTROL_MODE = 0;
Lightvalve 57:f4819de54e7a 18 uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin)
Lightvalve 52:8ea76864368a 19 uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure)
Lightvalve 57:f4819de54e7a 20 uint8_t CONTROL_UTILITY_MODE = 0;
Lightvalve 52:8ea76864368a 21 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
Lightvalve 52:8ea76864368a 22 uint8_t FLAG_VALVE_DEADZONE = 0;
Lightvalve 11:82d8768d7351 23 uint8_t REFERENCE_MODE = 0;
Lightvalve 54:647072f5307a 24 int16_t CAN_FREQ = 500;
Lightvalve 17:1865016ca2e7 25 int16_t DIR_JOINT_ENC = 0;
Lightvalve 17:1865016ca2e7 26 int16_t DIR_VALVE = 0;
Lightvalve 17:1865016ca2e7 27 int16_t DIR_VALVE_ENC = 0;
Lightvalve 11:82d8768d7351 28
Lightvalve 49:e7bcfc244d40 29 float SUPPLY_VOLTAGE = 12.0f;
Lightvalve 49:e7bcfc244d40 30 float VALVE_VOLTAGE_LIMIT = 12.0f; //v
Lightvalve 11:82d8768d7351 31
Lightvalve 30:8d561f16383b 32 float P_GAIN_VALVE_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 33 float I_GAIN_VALVE_POSITION= 0.0f;
Lightvalve 30:8d561f16383b 34 float D_GAIN_VALVE_POSITION= 0.0f;
Lightvalve 30:8d561f16383b 35 float P_GAIN_JOINT_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 36 float I_GAIN_JOINT_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 37 float D_GAIN_JOINT_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 38 float P_GAIN_JOINT_TORQUE = 0.0f;
Lightvalve 30:8d561f16383b 39 float I_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 30:8d561f16383b 40 float D_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 41
Lightvalve 46:2694daea349b 42 int16_t K_SPRING = 0.0;
Lightvalve 46:2694daea349b 43 int16_t D_DAMPER = 0.0;
Lightvalve 54:647072f5307a 44 int16_t flag_delay_test = 0;
Lightvalve 46:2694daea349b 45
Lightvalve 36:a46e63505ed8 46 //float P_GAIN_VALVE_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 47 //float I_GAIN_VALVE_POSITION_OPP= 0.0f;
Lightvalve 36:a46e63505ed8 48 //float D_GAIN_VALVE_POSITION_OPP= 0.0f;
Lightvalve 36:a46e63505ed8 49 //float P_GAIN_JOINT_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 50 //float I_GAIN_JOINT_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 51 //float D_GAIN_JOINT_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 52 //float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 53 //float I_GAIN_JOINT_TORQUE_OPP = 0.0;
Lightvalve 36:a46e63505ed8 54 //float D_GAIN_JOINT_TORQUE_OPP = 0.0;
Lightvalve 36:a46e63505ed8 55
Lightvalve 33:91b17819ec30 56 float VALVE_DEADZONE_PLUS;
Lightvalve 33:91b17819ec30 57 float VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 58
Lightvalve 11:82d8768d7351 59 int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 60 int16_t COMPLIANCE_GAIN;
Lightvalve 11:82d8768d7351 61
Lightvalve 11:82d8768d7351 62 int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 63
Lightvalve 11:82d8768d7351 64 int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 65
Lightvalve 11:82d8768d7351 66 int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 67
Lightvalve 11:82d8768d7351 68 int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 69 int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 70
Lightvalve 11:82d8768d7351 71 int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 72 int16_t PISTON_AREA_B;
Lightvalve 30:8d561f16383b 73 float PISTON_AREA_alpha;
Lightvalve 57:f4819de54e7a 74 float alpha3 = 1.0f;
Lightvalve 11:82d8768d7351 75
Lightvalve 11:82d8768d7351 76
Lightvalve 11:82d8768d7351 77 int16_t PRES_SUPPLY;
Lightvalve 11:82d8768d7351 78 int16_t PRES_RETURN;
Lightvalve 11:82d8768d7351 79
Lightvalve 11:82d8768d7351 80 int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 81 int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 82
Lightvalve 11:82d8768d7351 83 int16_t STROKE;
Lightvalve 11:82d8768d7351 84
Lightvalve 11:82d8768d7351 85
Lightvalve 57:f4819de54e7a 86 //int16_t VALVE_LIMIT_PLUS;
Lightvalve 57:f4819de54e7a 87 //int16_t VALVE_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 88
Lightvalve 48:889798ff9329 89 float ENC_PULSE_PER_POSITION;
Lightvalve 48:889798ff9329 90 float TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 30:8d561f16383b 91 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 92 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 11:82d8768d7351 93
Lightvalve 11:82d8768d7351 94 int HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 95 int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 96
Lightvalve 30:8d561f16383b 97 float FRICTION;
Lightvalve 30:8d561f16383b 98 float REF_PERIOD;
Lightvalve 30:8d561f16383b 99 float REF_MAG;
Lightvalve 11:82d8768d7351 100 int REF_NUM;
Lightvalve 11:82d8768d7351 101
Lightvalve 11:82d8768d7351 102
Lightvalve 30:8d561f16383b 103 float DAC_REF;
Lightvalve 30:8d561f16383b 104 float DAC_RESOL;
Lightvalve 11:82d8768d7351 105
Lightvalve 11:82d8768d7351 106 int REF_POSITION;
Lightvalve 11:82d8768d7351 107 int REF_VELOCITY;
Lightvalve 11:82d8768d7351 108 int16_t REF_TORQUE;
Lightvalve 11:82d8768d7351 109 int16_t REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 110 int16_t REF_PWM;
Lightvalve 11:82d8768d7351 111 int16_t REF_VALVE_POSITION;
Lightvalve 14:8e7590227d22 112 int16_t REF_CURRENT;
Lightvalve 11:82d8768d7351 113
Lightvalve 11:82d8768d7351 114 int REF_MOVE_TIME_5k;
Lightvalve 11:82d8768d7351 115 int INIT_REF_PWM;
Lightvalve 11:82d8768d7351 116 int INIT_REF_VALVE_POS;
Lightvalve 11:82d8768d7351 117 int INIT_REF_POS;
Lightvalve 11:82d8768d7351 118 int INIT_REF_VEL;
Lightvalve 11:82d8768d7351 119 int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 120 int INIT_REF_PRES_DIFF;
Lightvalve 14:8e7590227d22 121 int INIT_REF_CURRENT;
Lightvalve 11:82d8768d7351 122
Lightvalve 11:82d8768d7351 123 int CUR_POSITION;
Lightvalve 11:82d8768d7351 124 int CUR_VELOCITY;
Lightvalve 30:8d561f16383b 125 float CUR_TORQUE;
Lightvalve 30:8d561f16383b 126 float CUR_PRES_A;
Lightvalve 30:8d561f16383b 127 float CUR_PRES_B;
Lightvalve 11:82d8768d7351 128 int CUR_VALVE_POSITION;
Lightvalve 11:82d8768d7351 129
Lightvalve 11:82d8768d7351 130 unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 131 unsigned int TMR2_COUNT_LED2;
Lightvalve 54:647072f5307a 132 unsigned int TMR2_COUNT_CAN_TX = 0;
Lightvalve 54:647072f5307a 133 unsigned int TMR3_COUNT_TEST = 0;
Lightvalve 11:82d8768d7351 134
Lightvalve 11:82d8768d7351 135 int num_err;
Lightvalve 11:82d8768d7351 136 int flag_err[8];
Lightvalve 11:82d8768d7351 137 int flag_err_old[8];
Lightvalve 11:82d8768d7351 138 int flag_err_rt;
Lightvalve 11:82d8768d7351 139
Lightvalve 11:82d8768d7351 140 int flag_ref_enable;
Lightvalve 11:82d8768d7351 141
Lightvalve 11:82d8768d7351 142 int flag_data_request[5];
Lightvalve 11:82d8768d7351 143
Lightvalve 45:35fa6884d0c6 144 int MODE_POS_FT_TRANS = 0;
Lightvalve 45:35fa6884d0c6 145
Lightvalve 57:f4819de54e7a 146 float CUR_CURRENT_mA = 0.0f;
Lightvalve 57:f4819de54e7a 147 float CUR_PRES_A_BAR = 0.0f;
Lightvalve 57:f4819de54e7a 148 float CUR_PRES_B_BAR = 0.0f;
Lightvalve 57:f4819de54e7a 149 float CUR_TORQUE_NM = 0.0f;
Lightvalve 57:f4819de54e7a 150 float CUR_TORQUE_NM_PRESS = 0.0f;
Lightvalve 11:82d8768d7351 151
Lightvalve 57:f4819de54e7a 152 float PRES_A_VREF = 0.0f;
Lightvalve 57:f4819de54e7a 153 float PRES_B_VREF = 0.0f;
Lightvalve 57:f4819de54e7a 154 float TORQUE_VREF = 0.0f;
Lightvalve 11:82d8768d7351 155
Lightvalve 57:f4819de54e7a 156 float VALVE_PWM_RAW_FB = 0.0f;
Lightvalve 57:f4819de54e7a 157 float VALVE_PWM_RAW_FF = 0.0f;
Lightvalve 57:f4819de54e7a 158 float VALVE_PWM_RAW = 0.0f;
Lightvalve 57:f4819de54e7a 159 int VALVE_PWM_VALVE_DZ = 0;
Lightvalve 11:82d8768d7351 160
Lightvalve 30:8d561f16383b 161 float VALVE_GAIN_LPM_PER_V[10];
Lightvalve 30:8d561f16383b 162 float VALVE_POS_VS_PWM[25];
Lightvalve 11:82d8768d7351 163 long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 164
Lightvalve 11:82d8768d7351 165 int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 166 int VALVE_MIN_POS;
Lightvalve 11:82d8768d7351 167 int VALVE_POS_NUM;
Lightvalve 32:4b8c0fedaf2c 168 float VALVE_CENTER_OFFSET;
Lightvalve 33:91b17819ec30 169 float VALVE_DZ_MINUS_OFFSET;
Lightvalve 33:91b17819ec30 170 float VALVE_DZ_PLUS_OFFSET;
Lightvalve 11:82d8768d7351 171
Lightvalve 57:f4819de54e7a 172 int TMR3_COUNT_FINDHOME = 0;
Lightvalve 57:f4819de54e7a 173 int TMR3_COUNT_FLOWRATE = 0;
Lightvalve 57:f4819de54e7a 174 int TMR3_COUNT_DEADZONE = 0;
Lightvalve 57:f4819de54e7a 175 int TMR3_COUNT_PRES_NULL = 0;
Lightvalve 57:f4819de54e7a 176 int TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve 57:f4819de54e7a 177 int TMR3_COUNT_PRES_CALIB = 0;
Lightvalve 57:f4819de54e7a 178 int TMR3_COUNT_REFERENCE = 0;
Lightvalve 57:f4819de54e7a 179 int TMR3_COUNT_JOINT = 0;
Lightvalve 57:f4819de54e7a 180 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
Lightvalve 11:82d8768d7351 181
Lightvalve 57:f4819de54e7a 182 float TUNING_TIME = 600.0f; // sec
Lightvalve 56:6f50d9d3bfee 183
Lightvalve 57:f4819de54e7a 184 float REFERENCE_FREQ = 1.0f;
Lightvalve 57:f4819de54e7a 185 float REFERENCE_MAG = 0.0f;
Lightvalve 11:82d8768d7351 186
Lightvalve 11:82d8768d7351 187 bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 188
Lightvalve 11:82d8768d7351 189 int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 190 enum _JUMP_STATUS {
Lightvalve 11:82d8768d7351 191 JUMP_NO_ACT = 0, //0
Lightvalve 11:82d8768d7351 192 JUMP_START, //1
Lightvalve 11:82d8768d7351 193 JUMP_TAKEOFF, //2
Lightvalve 11:82d8768d7351 194 JUMP_FLYING, //3
Lightvalve 11:82d8768d7351 195 JUMP_LANDING, //4
Lightvalve 11:82d8768d7351 196 };
Lightvalve 11:82d8768d7351 197
Lightvalve 30:8d561f16383b 198 float CUR_PRES_DIFF_BAR = 0.0f;
Lightvalve 30:8d561f16383b 199 float CUR_PRES_A_sum = 0.0f;
Lightvalve 30:8d561f16383b 200 float CUR_PRES_B_sum = 0.0f;
Lightvalve 30:8d561f16383b 201 float CUR_PRES_A_mean = 0.0f;
Lightvalve 30:8d561f16383b 202 float CUR_PRES_B_mean = 0.0f;
Lightvalve 30:8d561f16383b 203 float CUR_TORQUE_sum = 0.0f;
Lightvalve 30:8d561f16383b 204 float CUR_TORQUE_mean = 0.0f;
Lightvalve 58:2eade98630e2 205 float PRES_A_NULL = 300.0f;
Lightvalve 58:2eade98630e2 206 float PRES_B_NULL = 300.0f;
Lightvalve 30:8d561f16383b 207 float TORQUE_NULL = 3900.0f;
Lightvalve 11:82d8768d7351 208
Lightvalve 30:8d561f16383b 209 float Ref_Valve_Pos_Old = 0.0f;
Lightvalve 11:82d8768d7351 210
Lightvalve 13:747daba9cf59 211 int VALVE_ID_timer = 0;
Lightvalve 13:747daba9cf59 212 int VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 213 int VALVE_FR_timer = 0;
Lightvalve 35:34ce7b0347b8 214 //int VALVE_HPL_timer = 0;
Lightvalve 13:747daba9cf59 215 int VALVE_POS_TMP = 0;
Lightvalve 13:747daba9cf59 216 int JOINT_VEL_TMP = 0;
Lightvalve 13:747daba9cf59 217 int DDV_POS_AVG = 0;
Lightvalve 19:23b7c1ad8683 218 int VALVE_POS_AVG[50] = {0};
Lightvalve 13:747daba9cf59 219 int VALVE_POS_AVG_OLD = 0;
Lightvalve 13:747daba9cf59 220 int data_num = 0;
Lightvalve 13:747daba9cf59 221 int ID_index = 0;
Lightvalve 13:747daba9cf59 222 int DZ_index = 1;
Lightvalve 19:23b7c1ad8683 223 int ID_index_array[50] = {0};
Lightvalve 13:747daba9cf59 224 int first_check = 0;
Lightvalve 30:8d561f16383b 225 float init_time = 0.0f;
Lightvalve 13:747daba9cf59 226 int VEL_POINT = 0;
Lightvalve 13:747daba9cf59 227 int DZ_case = 0;
Lightvalve 13:747daba9cf59 228 int START_POS = 0;
Lightvalve 13:747daba9cf59 229 int FINAL_POS = 0;
Lightvalve 13:747daba9cf59 230 int DZ_DIRECTION = 0;
Lightvalve 13:747daba9cf59 231 int FIRST_DZ = 0;
Lightvalve 13:747daba9cf59 232 int SECOND_DZ = 0;
Lightvalve 13:747daba9cf59 233 int DZ_NUM = 0;
Lightvalve 13:747daba9cf59 234 int one_period_end = 0;
Lightvalve 30:8d561f16383b 235 float Ref_Vel_Test = 0.0f;
Lightvalve 13:747daba9cf59 236 long TMR2_FOR_SLOW_LOGGING = 0;
Lightvalve 19:23b7c1ad8683 237 //int velcount = 0;
Lightvalve 13:747daba9cf59 238 char max_check = 0;
Lightvalve 34:bb2ca2fc2a8e 239 char min_check = 0;
Lightvalve 13:747daba9cf59 240
Lightvalve 30:8d561f16383b 241 float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f;
Lightvalve 30:8d561f16383b 242 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;
Lightvalve 30:8d561f16383b 243 float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
Lightvalve 30:8d561f16383b 244 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
Lightvalve 13:747daba9cf59 245
Lightvalve 30:8d561f16383b 246 float CUR_FLOWRATE = 0.0f;
Lightvalve 30:8d561f16383b 247 float VALVE_FF_VOLTAGE = 0.0f;
Lightvalve 13:747daba9cf59 248
Lightvalve 13:747daba9cf59 249 int pos_plus_end = 0;
Lightvalve 13:747daba9cf59 250 int pos_minus_end = 0;
Lightvalve 13:747daba9cf59 251
Lightvalve 13:747daba9cf59 252 bool need_enc_init = false;
Lightvalve 13:747daba9cf59 253
Lightvalve 13:747daba9cf59 254 int temp_time = 0;
Lightvalve 13:747daba9cf59 255
Lightvalve 30:8d561f16383b 256 float CUR_VELOCITY_sum = 0.0f;
Lightvalve 30:8d561f16383b 257 float temp_vel_sum = 0.0f;
Lightvalve 13:747daba9cf59 258
Lightvalve 13:747daba9cf59 259 int DZ_dir = 0;
Lightvalve 13:747daba9cf59 260 int DZ_temp_cnt = 0;
Lightvalve 13:747daba9cf59 261 int DZ_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 262 int DZ_end = 2;
Lightvalve 13:747daba9cf59 263 int flag_flowrate = 0;
Lightvalve 13:747daba9cf59 264 int fl_temp_cnt = 0;
Lightvalve 13:747daba9cf59 265 int fl_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 266 int cur_vel_sum = 0;
Lightvalve 13:747daba9cf59 267
Lightvalve 57:f4819de54e7a 268 float Cur_Valve_Open_pulse = 0.0f;
Lightvalve 57:f4819de54e7a 269
Lightvalve 13:747daba9cf59 270 // find home
Lightvalve 13:747daba9cf59 271 int CUR_VELOCITY_OLD = 0;
Lightvalve 13:747daba9cf59 272 int cnt_findhome = 0;
Lightvalve 13:747daba9cf59 273 int cnt_vel_findhome = 0;
Lightvalve 13:747daba9cf59 274 int FINDHOME_VELOCITY = 0;
Lightvalve 13:747daba9cf59 275 int FINDHOME_VELOCITY_OLD = 0;
Lightvalve 13:747daba9cf59 276 int FINDHOME_POSITION = 0;
Lightvalve 13:747daba9cf59 277 int FINDHOME_POSITION_OLD = 0;
Lightvalve 13:747daba9cf59 278
Lightvalve 13:747daba9cf59 279 // valve gain
Lightvalve 13:747daba9cf59 280 int check_vel_pos_init = 0;
Lightvalve 13:747daba9cf59 281 int check_vel_pos_fin = 0;
Lightvalve 13:747daba9cf59 282 int check_vel_pos_interv = 0;
Lightvalve 13:747daba9cf59 283 int valve_gain_repeat_cnt = 0;
Lightvalve 30:8d561f16383b 284 float VALVE_VOLTAGE = 0.0f;
Lightvalve 13:747daba9cf59 285
Lightvalve 30:8d561f16383b 286 float freq_fric_tune = 1.0f;
Lightvalve 13:747daba9cf59 287
Lightvalve 14:8e7590227d22 288 uint32_t TMR3_COUNT_CAN_TX = 0;
Lightvalve 14:8e7590227d22 289
Lightvalve 57:f4819de54e7a 290 // Current Control Variables
Lightvalve 57:f4819de54e7a 291 double I_REF = 0.0f;
Lightvalve 57:f4819de54e7a 292 double I_REF_fil = 0.0f;
Lightvalve 57:f4819de54e7a 293 double I_ERR = 0.0f;
Lightvalve 57:f4819de54e7a 294 double I_ERR_INT = 0.0f;
Lightvalve 57:f4819de54e7a 295 double I_REF_fil_old = 0.0f;
Lightvalve 57:f4819de54e7a 296 double I_REF_fil_diff = 0.0f;
Lightvalve 57:f4819de54e7a 297
Lightvalve 57:f4819de54e7a 298 // system id
Lightvalve 57:f4819de54e7a 299 int cnt_sysid = 0;
Lightvalve 57:f4819de54e7a 300 double freq_sysid_Iref = 0.0f;
Lightvalve 57:f4819de54e7a 301
Lightvalve 14:8e7590227d22 302 int TMR3_COUNT_IREF = 0;
Lightvalve 30:8d561f16383b 303 float CUR_CURRENT = 0.0f;
Lightvalve 30:8d561f16383b 304 float u_CUR[3] = {0.0f,0.0f,0.0f};
Lightvalve 29:69f3f5445d6d 305 int FINDHOME_STAGE = 0;
Lightvalve 29:69f3f5445d6d 306 int FINDHOME_INIT = 0;
Lightvalve 30:8d561f16383b 307 int FINDHOME_GOTOLIMIT = 1;
Lightvalve 30:8d561f16383b 308 int FINDHOME_ZEROPOSE = 2;
Lightvalve 14:8e7590227d22 309
Lightvalve 45:35fa6884d0c6 310 float alpha_trans = 0.0f;
Lightvalve 45:35fa6884d0c6 311
Lightvalve 57:f4819de54e7a 312 float V_out=0.0f;
Lightvalve 57:f4819de54e7a 313 float V_rem=0.0f; // for anti-windup
Lightvalve 57:f4819de54e7a 314 float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
Lightvalve 57:f4819de54e7a 315
Lightvalve 57:f4819de54e7a 316 float PWM_out=0.0f;
Lightvalve 57:f4819de54e7a 317
Lightvalve 57:f4819de54e7a 318 double K_v = 0.0f; // valve flowrate gain
Lightvalve 57:f4819de54e7a 319 double mV_PER_mA = 600.0f; // current >> voltage
Lightvalve 57:f4819de54e7a 320 double mV_PER_pulse = 0.6f; // pulse >> voltage
Lightvalve 57:f4819de54e7a 321 double mA_PER_pulse = 0.001f; // pulse >> current
Lightvalve 57:f4819de54e7a 322
Lightvalve 57:f4819de54e7a 323 int timer_while = 0;
Lightvalve 57:f4819de54e7a 324 int while_index = 0;
Lightvalve 57:f4819de54e7a 325
Lightvalve 23:59218d4a256d 326 //int h1, h2, h3, h4, h5, h6;
Lightvalve 23:59218d4a256d 327
Lightvalve 11:82d8768d7351 328 /*******************************************************************************
Lightvalve 11:82d8768d7351 329 * General math functions
Lightvalve 11:82d8768d7351 330 ******************************************************************************/
Lightvalve 11:82d8768d7351 331
Lightvalve 11:82d8768d7351 332
Lightvalve 34:bb2ca2fc2a8e 333 float dabs(float tx)
Lightvalve 34:bb2ca2fc2a8e 334 {
Lightvalve 30:8d561f16383b 335 if (tx >= 0.0f)
Lightvalve 11:82d8768d7351 336 return tx;
Lightvalve 11:82d8768d7351 337 else
Lightvalve 11:82d8768d7351 338 return -tx;
Lightvalve 11:82d8768d7351 339 }
Lightvalve 11:82d8768d7351 340
Lightvalve 34:bb2ca2fc2a8e 341 float change_int_to_efloat(int input)
Lightvalve 34:bb2ca2fc2a8e 342 {
Lightvalve 11:82d8768d7351 343 int i = 0;
Lightvalve 11:82d8768d7351 344
Lightvalve 30:8d561f16383b 345 float output = 0;
Lightvalve 48:889798ff9329 346 int vn = (int) ((float) input / 10.0f);
Lightvalve 11:82d8768d7351 347 int en = input % 10;
Lightvalve 11:82d8768d7351 348
Lightvalve 30:8d561f16383b 349 float temp = 1.;
Lightvalve 11:82d8768d7351 350 for (i = 0; i < en; i++)
Lightvalve 30:8d561f16383b 351 temp *= 0.1f;
Lightvalve 11:82d8768d7351 352
Lightvalve 30:8d561f16383b 353 output = (float) vn*temp;
Lightvalve 11:82d8768d7351 354 return output;
Lightvalve 11:82d8768d7351 355 }
Lightvalve 11:82d8768d7351 356
Lightvalve 34:bb2ca2fc2a8e 357 void make_delay(void)
Lightvalve 34:bb2ca2fc2a8e 358 {
Lightvalve 11:82d8768d7351 359 int i = 0;
Lightvalve 11:82d8768d7351 360
Lightvalve 11:82d8768d7351 361 for (i = 0; i < 100000; i++) {
Lightvalve 11:82d8768d7351 362 ;
Lightvalve 11:82d8768d7351 363 }
Lightvalve 11:82d8768d7351 364
Lightvalve 11:82d8768d7351 365 }
Lightvalve 11:82d8768d7351 366
Lightvalve 11:82d8768d7351 367
Lightvalve 11:82d8768d7351 368 /*******************************************************************************
Lightvalve 11:82d8768d7351 369 * ROM functions
Lightvalve 11:82d8768d7351 370 ******************************************************************************/
Lightvalve 34:bb2ca2fc2a8e 371 void ROM_RESET_DATA(void)
Lightvalve 34:bb2ca2fc2a8e 372 {
Lightvalve 16:903b5a4433b4 373 FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
Lightvalve 16:903b5a4433b4 374 if (!writer.ready()) writer.open();
Lightvalve 34:bb2ca2fc2a8e 375
Lightvalve 16:903b5a4433b4 376 writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀
Lightvalve 16:903b5a4433b4 377 writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE);
Lightvalve 52:8ea76864368a 378 writer.write(RID_SENSING_MODE, (int) SENSING_MODE);
Lightvalve 52:8ea76864368a 379 writer.write(RID_CURRENT_CONTROL_MODE, (int) CURRENT_CONTROL_MODE);
Lightvalve 52:8ea76864368a 380 writer.write(RID_FLAG_VALVE_DEADZONE, (int) FLAG_VALVE_DEADZONE);
Lightvalve 16:903b5a4433b4 381 writer.write(RID_CAN_FREQ,(int) CAN_FREQ);
Lightvalve 16:903b5a4433b4 382 writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC);
Lightvalve 16:903b5a4433b4 383 writer.write(RID_VALVE_DIR,(int) DIR_VALVE);
Lightvalve 16:903b5a4433b4 384 writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC);
Lightvalve 30:8d561f16383b 385 writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0f));
Lightvalve 30:8d561f16383b 386 writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0f));
Lightvalve 16:903b5a4433b4 387 writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 388 writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 389 writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 390 writer.write(RID_P_GAIN_JOINT_POSITION,(int) P_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 391 writer.write(RID_I_GAIN_JOINT_POSITION,(int) I_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 392 writer.write(RID_D_GAIN_JOINT_POSITION,(int) D_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 393 writer.write(RID_P_GAIN_JOINT_TORQUE,(int) P_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 394 writer.write(RID_I_GAIN_JOINT_TORQUE,(int) I_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 395 writer.write(RID_D_GAIN_JOINT_TORQUE,(int) D_GAIN_JOINT_TORQUE);
Lightvalve 33:91b17819ec30 396 writer.write(RID_VALVE_DEADZONE_PLUS,(int) (VALVE_DEADZONE_PLUS * 10.0f));
Lightvalve 33:91b17819ec30 397 writer.write(RID_VALVE_DEADZONE_MINUS,(int) (VALVE_DEADZONE_MINUS * 10.0f));
Lightvalve 16:903b5a4433b4 398 writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN);
Lightvalve 16:903b5a4433b4 399 writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN);
Lightvalve 16:903b5a4433b4 400 writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER);
Lightvalve 16:903b5a4433b4 401 writer.write(RID_VALVE_FF,(int) VALVE_FF);
Lightvalve 16:903b5a4433b4 402 writer.write(RID_BULK_MODULUS,(int) BNO);
Lightvalve 16:903b5a4433b4 403 writer.write(RID_CHAMBER_VOLUME_A,(int) CHAMBER_VOLUME_A);
Lightvalve 16:903b5a4433b4 404 writer.write(RID_CHAMBER_VOLUME_B,(int) CHAMBER_VOLUME_B);
Lightvalve 16:903b5a4433b4 405 writer.write(RID_PISTON_AREA_A,(int) PISTON_AREA_A);
Lightvalve 16:903b5a4433b4 406 writer.write(RID_PISTON_AREA_B,(int) PISTON_AREA_B);
Lightvalve 16:903b5a4433b4 407 writer.write(RID_PRES_SUPPLY,(int) PRES_SUPPLY);
Lightvalve 16:903b5a4433b4 408 writer.write(RID_PRES_RETURN,(int) PRES_RETURN);
Lightvalve 16:903b5a4433b4 409 writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS);
Lightvalve 16:903b5a4433b4 410 writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS);
Lightvalve 16:903b5a4433b4 411 writer.write(RID_STROKE,(int) STROKE);
Lightvalve 57:f4819de54e7a 412 //writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
Lightvalve 57:f4819de54e7a 413 //writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
Lightvalve 48:889798ff9329 414 writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f));
Lightvalve 58:2eade98630e2 415 writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f));
Lightvalve 30:8d561f16383b 416 writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
Lightvalve 30:8d561f16383b 417 writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 418 writer.write(RID_FRICTION,(int) (FRICTION * 10.0f));
Lightvalve 34:bb2ca2fc2a8e 419 writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET);
Lightvalve 34:bb2ca2fc2a8e 420 writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING);
Lightvalve 34:bb2ca2fc2a8e 421 writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f));
Lightvalve 30:8d561f16383b 422 writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f));
Lightvalve 30:8d561f16383b 423 writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f));
Lightvalve 34:bb2ca2fc2a8e 424 writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 425 writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 426 writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 427 writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 428 writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 429 writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 430 writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 431 writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 432 writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
Lightvalve 30:8d561f16383b 433 writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 434 for(int i=0; i<25; i++) {
Lightvalve 18:b8adf1582ea3 435 writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]);
Lightvalve 18:b8adf1582ea3 436 }
Lightvalve 34:bb2ca2fc2a8e 437 for(int i=0; i<100; i++) {
Lightvalve 19:23b7c1ad8683 438 writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF));
Lightvalve 19:23b7c1ad8683 439 writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF));
Lightvalve 18:b8adf1582ea3 440 }
Lightvalve 18:b8adf1582ea3 441 writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS);
Lightvalve 18:b8adf1582ea3 442 writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS);
Lightvalve 57:f4819de54e7a 443 //writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
Lightvalve 18:b8adf1582ea3 444 writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM);
Lightvalve 46:2694daea349b 445
Lightvalve 46:2694daea349b 446 writer.write(RID_K_SPRING, (int) K_SPRING);
Lightvalve 46:2694daea349b 447 writer.write(RID_D_DAMPER, (int) D_DAMPER);
Lightvalve 11:82d8768d7351 448
Lightvalve 34:bb2ca2fc2a8e 449 writer.close();
Lightvalve 34:bb2ca2fc2a8e 450
Lightvalve 11:82d8768d7351 451 }
Lightvalve 11:82d8768d7351 452
Lightvalve 34:bb2ca2fc2a8e 453 void ROM_CALL_DATA(void)
Lightvalve 34:bb2ca2fc2a8e 454 {
Lightvalve 16:903b5a4433b4 455 BNO = flashReadInt(Rom_Sector, RID_BNO);
Lightvalve 57:f4819de54e7a 456 BNO = 1;
Lightvalve 16:903b5a4433b4 457 OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
Lightvalve 52:8ea76864368a 458 SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
Lightvalve 52:8ea76864368a 459 CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE);
Lightvalve 52:8ea76864368a 460 FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE);
Lightvalve 16:903b5a4433b4 461 CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
Lightvalve 16:903b5a4433b4 462 DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR);
Lightvalve 16:903b5a4433b4 463 DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
Lightvalve 16:903b5a4433b4 464 DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
Lightvalve 30:8d561f16383b 465 SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
Lightvalve 30:8d561f16383b 466 VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
Lightvalve 16:903b5a4433b4 467 P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 468 I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 469 D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 470 P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 471 I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 472 D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 473 P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 474 I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 475 D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE);
Lightvalve 33:91b17819ec30 476 VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f;
Lightvalve 33:91b17819ec30 477 VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
Lightvalve 16:903b5a4433b4 478 VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
Lightvalve 16:903b5a4433b4 479 COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
Lightvalve 16:903b5a4433b4 480 VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
Lightvalve 16:903b5a4433b4 481 VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
Lightvalve 16:903b5a4433b4 482 BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);
Lightvalve 16:903b5a4433b4 483 CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A);
Lightvalve 16:903b5a4433b4 484 CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
Lightvalve 16:903b5a4433b4 485 PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
Lightvalve 16:903b5a4433b4 486 PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
Lightvalve 30:8d561f16383b 487 PISTON_AREA_alpha = (float)PISTON_AREA_B/(float)PISTON_AREA_A;
Lightvalve 57:f4819de54e7a 488 alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
Lightvalve 16:903b5a4433b4 489 PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
Lightvalve 16:903b5a4433b4 490 PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
Lightvalve 16:903b5a4433b4 491 ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
Lightvalve 16:903b5a4433b4 492 ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
Lightvalve 16:903b5a4433b4 493 STROKE = flashReadInt(Rom_Sector, RID_STROKE);
Lightvalve 57:f4819de54e7a 494 //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
Lightvalve 57:f4819de54e7a 495 //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
Lightvalve 48:889798ff9329 496 ENC_PULSE_PER_POSITION = (float) (flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION)) * 0.1f;
Lightvalve 48:889798ff9329 497 // ENC_PULSE_PER_POSITION = (float) 1024.0f;
Lightvalve 58:2eade98630e2 498 TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
Lightvalve 48:889798ff9329 499 //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
Lightvalve 48:889798ff9329 500 // TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
Lightvalve 30:8d561f16383b 501 PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
Lightvalve 48:889798ff9329 502 // PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 503 PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
Lightvalve 48:889798ff9329 504 // PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 505 FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
Lightvalve 16:903b5a4433b4 506 HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
Lightvalve 16:903b5a4433b4 507 HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
Lightvalve 30:8d561f16383b 508 TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
Lightvalve 32:4b8c0fedaf2c 509 PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f;
Lightvalve 32:4b8c0fedaf2c 510 PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f;
Lightvalve 30:8d561f16383b 511 VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
Lightvalve 30:8d561f16383b 512 VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
Lightvalve 30:8d561f16383b 513 VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
Lightvalve 30:8d561f16383b 514 VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
Lightvalve 30:8d561f16383b 515 VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
Lightvalve 30:8d561f16383b 516 VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
Lightvalve 30:8d561f16383b 517 VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
Lightvalve 30:8d561f16383b 518 VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
Lightvalve 30:8d561f16383b 519 VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
Lightvalve 30:8d561f16383b 520 VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
Lightvalve 34:bb2ca2fc2a8e 521 for(int i=0; i<25; i++) {
Lightvalve 30:8d561f16383b 522 VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
Lightvalve 16:903b5a4433b4 523 }
Lightvalve 34:bb2ca2fc2a8e 524 for(int i=0; i<100; i++) {
Lightvalve 16:903b5a4433b4 525 JOINT_VEL[i] = ( ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
Lightvalve 16:903b5a4433b4 526 }
Lightvalve 16:903b5a4433b4 527 VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
Lightvalve 16:903b5a4433b4 528 VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
Lightvalve 57:f4819de54e7a 529 //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
Lightvalve 16:903b5a4433b4 530 VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
Lightvalve 46:2694daea349b 531
Lightvalve 46:2694daea349b 532 K_SPRING = flashReadInt(Rom_Sector, RID_K_SPRING);
Lightvalve 46:2694daea349b 533 D_DAMPER = flashReadInt(Rom_Sector, RID_D_DAMPER);
Lightvalve 48:889798ff9329 534
Lightvalve 11:82d8768d7351 535 }
Lightvalve 11:82d8768d7351 536
Lightvalve 11:82d8768d7351 537 /*******************************************************************************
Lightvalve 11:82d8768d7351 538 * ENCODER functions
Lightvalve 11:82d8768d7351 539 ******************************************************************************/
Lightvalve 11:82d8768d7351 540 // A-KHA
Lightvalve 30:8d561f16383b 541 #define KF_G1_11 0.083920206005350f
Lightvalve 30:8d561f16383b 542 #define KF_G1_12 0.000013905329560f
Lightvalve 30:8d561f16383b 543 #define KF_G1_21 -0.000575742328210f
Lightvalve 30:8d561f16383b 544 #define KF_G1_22 0.799999939711725f
Lightvalve 11:82d8768d7351 545 // K
Lightvalve 30:8d561f16383b 546 #define KF_G2_11 0.916079793994650f
Lightvalve 30:8d561f16383b 547 #define KF_G2_12 0.000002878711641f
Lightvalve 30:8d561f16383b 548 #define KF_G2_21 0.000575742328210f
Lightvalve 30:8d561f16383b 549 #define KF_G2_22 0.199999945139809f
Lightvalve 11:82d8768d7351 550
Lightvalve 30:8d561f16383b 551 float KF_Y_11 = 0.0f;
Lightvalve 30:8d561f16383b 552 float KF_Y_21 = 0.0f;
Lightvalve 30:8d561f16383b 553 float KF_X_11 = 0.0f;
Lightvalve 30:8d561f16383b 554 float KF_X_21 = 0.0f;
Lightvalve 11:82d8768d7351 555
Lightvalve 11:82d8768d7351 556 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
Lightvalve 11:82d8768d7351 557 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
Lightvalve 11:82d8768d7351 558 long enc_offset = 0;
Lightvalve 11:82d8768d7351 559
Lightvalve 34:bb2ca2fc2a8e 560 void ENC_UPDATE(void)
Lightvalve 34:bb2ca2fc2a8e 561 {
Lightvalve 11:82d8768d7351 562
Lightvalve 11:82d8768d7351 563 ENC_pos_cur = spi_enc_read();
Lightvalve 11:82d8768d7351 564 ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
Lightvalve 11:82d8768d7351 565
Lightvalve 57:f4819de54e7a 566 //Kalman Filter
Lightvalve 57:f4819de54e7a 567 // ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
Lightvalve 54:647072f5307a 568 // KF_Y_11 = ENC_pos_cur;
Lightvalve 54:647072f5307a 569 // KF_Y_21 = ENC_VEL_RAW;
Lightvalve 54:647072f5307a 570 // KF_X_11 = KF_G1_11 * KF_X_11 + KF_G1_12 * KF_X_21 + KF_G2_11 * KF_Y_11 + KF_G2_12*KF_Y_21;
Lightvalve 54:647072f5307a 571 // KF_X_21 = KF_G1_21 * KF_X_11 + KF_G1_22 * KF_X_21 + KF_G2_21 * KF_Y_11 + KF_G2_22*KF_Y_21;
Lightvalve 54:647072f5307a 572 // ENC_VEL_KF = (int32_t) KF_X_21;
Lightvalve 54:647072f5307a 573 //
Lightvalve 54:647072f5307a 574 // pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
Lightvalve 54:647072f5307a 575 // vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
Lightvalve 57:f4819de54e7a 576
Lightvalve 57:f4819de54e7a 577 //Low Pass Filter
Lightvalve 57:f4819de54e7a 578
Lightvalve 54:647072f5307a 579 double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset);
Lightvalve 57:f4819de54e7a 580 double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k);
Lightvalve 54:647072f5307a 581
Lightvalve 57:f4819de54e7a 582 double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
Lightvalve 54:647072f5307a 583 pos.sen = NEW_POSITION;
Lightvalve 57:f4819de54e7a 584 vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
Lightvalve 57:f4819de54e7a 585
Lightvalve 11:82d8768d7351 586 ENC_pos_old = ENC_pos_cur;
Lightvalve 11:82d8768d7351 587 }
Lightvalve 11:82d8768d7351 588
Lightvalve 34:bb2ca2fc2a8e 589 void ENC_SET_ZERO(void)
Lightvalve 34:bb2ca2fc2a8e 590 {
Lightvalve 11:82d8768d7351 591
Lightvalve 11:82d8768d7351 592 spi_enc_set_clear();
Lightvalve 11:82d8768d7351 593 CUR_POSITION = 0;
Lightvalve 11:82d8768d7351 594 ENC_pos_old = ENC_pos_cur = 0;
Lightvalve 11:82d8768d7351 595
Lightvalve 11:82d8768d7351 596 }
Lightvalve 11:82d8768d7351 597
Lightvalve 34:bb2ca2fc2a8e 598 void ENC_SET(int32_t value_e)
Lightvalve 34:bb2ca2fc2a8e 599 {
Lightvalve 11:82d8768d7351 600 spi_enc_set_clear();
Lightvalve 13:747daba9cf59 601 enc_offset = value_e;
Lightvalve 13:747daba9cf59 602 CUR_POSITION = value_e;
Lightvalve 13:747daba9cf59 603 ENC_pos_old = ENC_pos_cur = value_e;
Lightvalve 11:82d8768d7351 604
Lightvalve 11:82d8768d7351 605 }
Lightvalve 18:b8adf1582ea3 606
Lightvalve 19:23b7c1ad8683 607
Lightvalve 30:8d561f16383b 608
Lightvalve 57:f4819de54e7a 609