Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_PostLIGHT_210420

LIGHT2

Dependencies:   FastPWM mbed

function_utilities/function_utilities.cpp@245:e9c5ec04e378, 2022-06-17 (annotated)

Committer:
Lightvalve
Date:
Fri Jun 17 06:06:53 2022 +0000
Revision:
245:e9c5ec04e378
Parent:
244:30896263bd8b 
220617

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 11:82d8768d7351 8 /*******************************************************************************
Lightvalve 11:82d8768d7351 9 * VARIABLE
Lightvalve 11:82d8768d7351 10 ******************************************************************************/
Lightvalve 11:82d8768d7351 11
Lightvalve 34:bb2ca2fc2a8e 12 // Board Information
Lightvalve 11:82d8768d7351 13 uint8_t BNO = 0;
Lightvalve 11:82d8768d7351 14 uint8_t CONTROL_MODE = 0;
Lightvalve 57:f4819de54e7a 15 uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin)
Lightvalve 52:8ea76864368a 16 uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure)
jobuuu 230:2c3e5ecbe7e1 17 uint8_t SUPPLY_PRESSURE_UPDATE = 0; // (0 : Update Off (constant Ps) , 1 : Update On (variable Ps))
jobuuu 230:2c3e5ecbe7e1 18
Lightvalve 57:f4819de54e7a 19 uint8_t CONTROL_UTILITY_MODE = 0;
Lightvalve 52:8ea76864368a 20 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
Lightvalve 52:8ea76864368a 21 uint8_t FLAG_VALVE_DEADZONE = 0;
hyhoon 227:699c3e572283 22 uint8_t REFERENCE_MODE = 1;
Lightvalve 54:647072f5307a 23 int16_t CAN_FREQ = 500;
Lightvalve 245:e9c5ec04e378 24 int16_t DIR_JOINT_ENC = 1;
Lightvalve 17:1865016ca2e7 25 int16_t DIR_VALVE = 0;
Lightvalve 17:1865016ca2e7 26 int16_t DIR_VALVE_ENC = 0;
Lightvalve 11:82d8768d7351 27
Lightvalve 49:e7bcfc244d40 28 float SUPPLY_VOLTAGE = 12.0f;
Lightvalve 49:e7bcfc244d40 29 float VALVE_VOLTAGE_LIMIT = 12.0f; //v
Lightvalve 11:82d8768d7351 30
Lightvalve 30:8d561f16383b 31 float P_GAIN_VALVE_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 32 float I_GAIN_VALVE_POSITION= 0.0f;
Lightvalve 30:8d561f16383b 33 float D_GAIN_VALVE_POSITION= 0.0f;
Lightvalve 30:8d561f16383b 34 float P_GAIN_JOINT_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 35 float I_GAIN_JOINT_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 36 float D_GAIN_JOINT_POSITION = 0.0f;
Lightvalve 30:8d561f16383b 37 float P_GAIN_JOINT_TORQUE = 0.0f;
Lightvalve 72:3436ce769b1e 38 float I_GAIN_JOINT_TORQUE = 0.0f;
Lightvalve 72:3436ce769b1e 39 float D_GAIN_JOINT_TORQUE = 0.0f;
Lightvalve 72:3436ce769b1e 40 float P_GAIN_JOINT_TORQUE_FF = 0.0f;
Lightvalve 72:3436ce769b1e 41 float I_GAIN_JOINT_TORQUE_FF = 0.0f;
Lightvalve 72:3436ce769b1e 42 float D_GAIN_JOINT_TORQUE_FF = 0.0f;
Lightvalve 11:82d8768d7351 43
Lightvalve 239:8ac5c6162bc1 44 int16_t K_SPRING = 0;
Lightvalve 239:8ac5c6162bc1 45 int16_t D_DAMPER = 0;
Lightvalve 72:3436ce769b1e 46
Lightvalve 54:647072f5307a 47 int16_t flag_delay_test = 0;
Lightvalve 46:2694daea349b 48
hyhoon 226:82a3ca333004 49 int16_t VALVE_DEADZONE_PLUS;
hyhoon 226:82a3ca333004 50 int16_t VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 51
Lightvalve 11:82d8768d7351 52 int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 53
Lightvalve 11:82d8768d7351 54 int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 55
Lightvalve 11:82d8768d7351 56 int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 57
Lightvalve 11:82d8768d7351 58 int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 59
Lightvalve 11:82d8768d7351 60 int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 61 int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 62
Lightvalve 11:82d8768d7351 63 int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 64 int16_t PISTON_AREA_B;
Lightvalve 30:8d561f16383b 65 float PISTON_AREA_alpha;
Lightvalve 57:f4819de54e7a 66 float alpha3 = 1.0f;
Lightvalve 11:82d8768d7351 67
jobuuu 230:2c3e5ecbe7e1 68 float PRES_SUPPLY_NOM = 70.0f;
jobuuu 230:2c3e5ecbe7e1 69 float PRES_SUPPLY = 70.0f;
Lightvalve 11:82d8768d7351 70
Lightvalve 11:82d8768d7351 71 int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 72 int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 73
Lightvalve 11:82d8768d7351 74 int16_t STROKE;
Lightvalve 11:82d8768d7351 75
Lightvalve 133:22ab22818e01 76 float Amm = 236.4f;
Lightvalve 133:22ab22818e01 77 float beta = 1300000000.0f;
Lightvalve 133:22ab22818e01 78 float Ps = 10000000.0f; //100bar = 100*10^5 Pa
Lightvalve 133:22ab22818e01 79 float Pt = 0.0f; // 0bar = 0Pa
Lightvalve 133:22ab22818e01 80 //float Kv = 0.00000002635f; // Q = Kv*xv*sqrt(Ps-Pa) => 100bar full opening 5LPM (full opening : xv = 1) [unit] m^3.5/kg^0.5
Lightvalve 133:22ab22818e01 81 float gamma_hat = 1075.0f; // Kv*beta*A/(sqrt(2)*V) 0.00000002635f * 1300000000.0f * / (sqrt(2.0f)*(1256.6f + 236.4f * 39.75f) * 0.000000001f / 2) [unit] m^3.5/kg^0.5
Lightvalve 218:066030f7951f 82 float a_hat = -13707631.7f;
Lightvalve 170:42c938a40313 83 float V_adapt = 0.0000053f; // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
Lightvalve 133:22ab22818e01 84 float x_4_des_old = 0.0f;
Lightvalve 11:82d8768d7351 85
jobuuu 230:2c3e5ecbe7e1 86 float ENC_PULSE_PER_POSITION = 1.0f;
jobuuu 230:2c3e5ecbe7e1 87 float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f;
Lightvalve 30:8d561f16383b 88 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 89 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 11:82d8768d7351 90
hyhoon 226:82a3ca333004 91 int16_t HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 92 int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 93
Lightvalve 30:8d561f16383b 94 float FRICTION;
Lightvalve 30:8d561f16383b 95 float REF_PERIOD;
Lightvalve 30:8d561f16383b 96 float REF_MAG;
Lightvalve 11:82d8768d7351 97 int REF_NUM;
Lightvalve 11:82d8768d7351 98
Lightvalve 11:82d8768d7351 99
Lightvalve 30:8d561f16383b 100 float DAC_REF;
Lightvalve 30:8d561f16383b 101 float DAC_RESOL;
Lightvalve 11:82d8768d7351 102
jobuuu 230:2c3e5ecbe7e1 103 float REF_FORCE = 0.0;
jobuuu 230:2c3e5ecbe7e1 104 float REF_TORQUE = 0.0;
jobuuu 230:2c3e5ecbe7e1 105 float REF_POSITION = 0.0;
jobuuu 230:2c3e5ecbe7e1 106 float REF_VELOCITY = 0.0;
jobuuu 230:2c3e5ecbe7e1 107
jobuuu 230:2c3e5ecbe7e1 108 float REF_POSITION_FINDHOME = 0.0;
jobuuu 230:2c3e5ecbe7e1 109
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_VEL;
Lightvalve 11:82d8768d7351 118 int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 119 int INIT_REF_PRES_DIFF;
Lightvalve 14:8e7590227d22 120 int INIT_REF_CURRENT;
Lightvalve 11:82d8768d7351 121
Lightvalve 11:82d8768d7351 122 unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 123 unsigned int TMR2_COUNT_LED2;
Lightvalve 54:647072f5307a 124 unsigned int TMR2_COUNT_CAN_TX = 0;
Lightvalve 54:647072f5307a 125 unsigned int TMR3_COUNT_TEST = 0;
Lightvalve 11:82d8768d7351 126
Lightvalve 11:82d8768d7351 127 int num_err;
Lightvalve 11:82d8768d7351 128 int flag_err[8];
Lightvalve 11:82d8768d7351 129 int flag_err_old[8];
Lightvalve 11:82d8768d7351 130 int flag_err_rt;
Lightvalve 11:82d8768d7351 131
Lightvalve 11:82d8768d7351 132 int flag_ref_enable;
Lightvalve 11:82d8768d7351 133
Lightvalve 11:82d8768d7351 134 int flag_data_request[5];
Lightvalve 11:82d8768d7351 135
Lightvalve 45:35fa6884d0c6 136 int MODE_POS_FT_TRANS = 0;
Lightvalve 66:a8e6799dbce3 137 int NN_Control_Flag = 0;
Lightvalve 45:35fa6884d0c6 138
Lightvalve 169:645207e160ca 139 int cnt_buffer = 0;
Lightvalve 169:645207e160ca 140
Lightvalve 57:f4819de54e7a 141 float CUR_CURRENT_mA = 0.0f;
Lightvalve 57:f4819de54e7a 142 float CUR_TORQUE_NM = 0.0f;
Lightvalve 57:f4819de54e7a 143 float CUR_TORQUE_NM_PRESS = 0.0f;
Lightvalve 11:82d8768d7351 144
jobuuu 230:2c3e5ecbe7e1 145 float FORCE_VREF = 0.0f;
Lightvalve 57:f4819de54e7a 146 float PRES_A_VREF = 0.0f;
Lightvalve 57:f4819de54e7a 147 float PRES_B_VREF = 0.0f;
Lightvalve 11:82d8768d7351 148
Lightvalve 57:f4819de54e7a 149 float VALVE_PWM_RAW_FB = 0.0f;
Lightvalve 57:f4819de54e7a 150 float VALVE_PWM_RAW_FF = 0.0f;
Lightvalve 57:f4819de54e7a 151 float VALVE_PWM_RAW = 0.0f;
Lightvalve 57:f4819de54e7a 152 int VALVE_PWM_VALVE_DZ = 0;
Lightvalve 11:82d8768d7351 153
Lightvalve 30:8d561f16383b 154 float VALVE_GAIN_LPM_PER_V[10];
Lightvalve 30:8d561f16383b 155 float VALVE_POS_VS_PWM[25];
Lightvalve 11:82d8768d7351 156 long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 157
Lightvalve 11:82d8768d7351 158 int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 159 int VALVE_MIN_POS;
Lightvalve 237:c2cfe349f5c1 160 int VALVE_ELECTRIC_CENTER;
Lightvalve 11:82d8768d7351 161 int VALVE_POS_NUM;
Lightvalve 32:4b8c0fedaf2c 162 float VALVE_CENTER_OFFSET;
Lightvalve 33:91b17819ec30 163 float VALVE_DZ_MINUS_OFFSET;
Lightvalve 33:91b17819ec30 164 float VALVE_DZ_PLUS_OFFSET;
Lightvalve 11:82d8768d7351 165
Lightvalve 57:f4819de54e7a 166 int TMR3_COUNT_FINDHOME = 0;
Lightvalve 57:f4819de54e7a 167 int TMR3_COUNT_FLOWRATE = 0;
Lightvalve 57:f4819de54e7a 168 int TMR3_COUNT_DEADZONE = 0;
Lightvalve 57:f4819de54e7a 169 int TMR3_COUNT_PRES_NULL = 0;
Lightvalve 57:f4819de54e7a 170 int TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve 57:f4819de54e7a 171 int TMR3_COUNT_PRES_CALIB = 0;
Lightvalve 57:f4819de54e7a 172 int TMR3_COUNT_REFERENCE = 0;
Lightvalve 57:f4819de54e7a 173 int TMR3_COUNT_JOINT = 0;
Lightvalve 57:f4819de54e7a 174 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
Lightvalve 11:82d8768d7351 175
Lightvalve 57:f4819de54e7a 176 float TUNING_TIME = 600.0f; // sec
Lightvalve 56:6f50d9d3bfee 177
Lightvalve 57:f4819de54e7a 178 float REFERENCE_FREQ = 1.0f;
Lightvalve 57:f4819de54e7a 179 float REFERENCE_MAG = 0.0f;
Lightvalve 11:82d8768d7351 180
Lightvalve 11:82d8768d7351 181 bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 182
Lightvalve 11:82d8768d7351 183 int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 184 enum _JUMP_STATUS {
Lightvalve 11:82d8768d7351 185 JUMP_NO_ACT = 0, //0
Lightvalve 11:82d8768d7351 186 JUMP_START, //1
Lightvalve 11:82d8768d7351 187 JUMP_TAKEOFF, //2
Lightvalve 11:82d8768d7351 188 JUMP_FLYING, //3
Lightvalve 11:82d8768d7351 189 JUMP_LANDING, //4
Lightvalve 11:82d8768d7351 190 };
Lightvalve 11:82d8768d7351 191
jobuuu 230:2c3e5ecbe7e1 192
jobuuu 230:2c3e5ecbe7e1 193 float PRES_A_NULL_pulse = 300.0f;
jobuuu 230:2c3e5ecbe7e1 194 float PRES_B_NULL_pulse = 300.0f;
jobuuu 230:2c3e5ecbe7e1 195 float FORCE_NULL_pulse = 3900.0f;
Lightvalve 11:82d8768d7351 196
Lightvalve 30:8d561f16383b 197 float Ref_Valve_Pos_Old = 0.0f;
Lightvalve 11:82d8768d7351 198
Lightvalve 13:747daba9cf59 199 int VALVE_ID_timer = 0;
Lightvalve 13:747daba9cf59 200 int VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 201 int VALVE_FR_timer = 0;
Lightvalve 35:34ce7b0347b8 202 //int VALVE_HPL_timer = 0;
Lightvalve 13:747daba9cf59 203 int VALVE_POS_TMP = 0;
Lightvalve 13:747daba9cf59 204 int JOINT_VEL_TMP = 0;
Lightvalve 13:747daba9cf59 205 int DDV_POS_AVG = 0;
Lightvalve 19:23b7c1ad8683 206 int VALVE_POS_AVG[50] = {0};
Lightvalve 13:747daba9cf59 207 int VALVE_POS_AVG_OLD = 0;
Lightvalve 13:747daba9cf59 208 int data_num = 0;
Lightvalve 13:747daba9cf59 209 int ID_index = 0;
Lightvalve 13:747daba9cf59 210 int DZ_index = 1;
Lightvalve 19:23b7c1ad8683 211 int ID_index_array[50] = {0};
Lightvalve 13:747daba9cf59 212 int first_check = 0;
Lightvalve 30:8d561f16383b 213 float init_time = 0.0f;
Lightvalve 13:747daba9cf59 214 int DZ_case = 0;
Lightvalve 13:747daba9cf59 215 int START_POS = 0;
Lightvalve 13:747daba9cf59 216 int FINAL_POS = 0;
Lightvalve 13:747daba9cf59 217 int DZ_DIRECTION = 0;
Lightvalve 13:747daba9cf59 218 int FIRST_DZ = 0;
Lightvalve 13:747daba9cf59 219 int SECOND_DZ = 0;
Lightvalve 13:747daba9cf59 220 int DZ_NUM = 0;
Lightvalve 13:747daba9cf59 221 int one_period_end = 0;
Lightvalve 30:8d561f16383b 222 float Ref_Vel_Test = 0.0f;
Lightvalve 13:747daba9cf59 223 char max_check = 0;
Lightvalve 34:bb2ca2fc2a8e 224 char min_check = 0;
Lightvalve 13:747daba9cf59 225
Lightvalve 30:8d561f16383b 226 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 227 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 228 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 229 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
Lightvalve 239:8ac5c6162bc1 230 float force_ref_filter = 0.0f;
Lightvalve 13:747daba9cf59 231
Lightvalve 30:8d561f16383b 232 float CUR_FLOWRATE = 0.0f;
Lightvalve 30:8d561f16383b 233 float VALVE_FF_VOLTAGE = 0.0f;
Lightvalve 13:747daba9cf59 234
Lightvalve 13:747daba9cf59 235 int pos_plus_end = 0;
Lightvalve 13:747daba9cf59 236 int pos_minus_end = 0;
Lightvalve 13:747daba9cf59 237
Lightvalve 13:747daba9cf59 238 bool need_enc_init = false;
Lightvalve 13:747daba9cf59 239
Lightvalve 13:747daba9cf59 240 int temp_time = 0;
Lightvalve 13:747daba9cf59 241
Lightvalve 30:8d561f16383b 242 float CUR_VELOCITY_sum = 0.0f;
Lightvalve 30:8d561f16383b 243 float temp_vel_sum = 0.0f;
Lightvalve 13:747daba9cf59 244
Lightvalve 13:747daba9cf59 245 int DZ_dir = 0;
Lightvalve 13:747daba9cf59 246 int DZ_temp_cnt = 0;
Lightvalve 13:747daba9cf59 247 int DZ_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 248 int DZ_end = 2;
Lightvalve 13:747daba9cf59 249 int flag_flowrate = 0;
Lightvalve 13:747daba9cf59 250 int fl_temp_cnt = 0;
Lightvalve 13:747daba9cf59 251 int fl_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 252 int cur_vel_sum = 0;
Lightvalve 13:747daba9cf59 253
hyhoon 227:699c3e572283 254 int cnt_finddz = 0;
hyhoon 227:699c3e572283 255 int cnt_vel_finddz = 0;
hyhoon 227:699c3e572283 256 int flag_finddz = 0;
hyhoon 227:699c3e572283 257 int FINDDZ_VELOCITY = 0;
hyhoon 227:699c3e572283 258 int FINDDZ_VELOCITY_OLD = 0;
hyhoon 227:699c3e572283 259 int FINDDZ_POSITION = 0;
hyhoon 227:699c3e572283 260 int FINDDZ_POSITION_OLD = 0;
hyhoon 227:699c3e572283 261
hyhoon 227:699c3e572283 262 double temp_VALVE_DEADZONE_PLUS = 0.0f;
hyhoon 227:699c3e572283 263 double temp_VALVE_DEADZONE_MINUS = 0.0f;
hyhoon 227:699c3e572283 264 float temp_pos_ref = 0.0f;
hyhoon 227:699c3e572283 265 float temp_pos_ref_offset = 0.0f;
hyhoon 227:699c3e572283 266
hyhoon 227:699c3e572283 267
Lightvalve 13:747daba9cf59 268 // valve gain
Lightvalve 13:747daba9cf59 269 int check_vel_pos_init = 0;
Lightvalve 13:747daba9cf59 270 int check_vel_pos_fin = 0;
Lightvalve 13:747daba9cf59 271 int check_vel_pos_interv = 0;
Lightvalve 13:747daba9cf59 272 int valve_gain_repeat_cnt = 0;
Lightvalve 30:8d561f16383b 273 float VALVE_VOLTAGE = 0.0f;
Lightvalve 13:747daba9cf59 274
Lightvalve 30:8d561f16383b 275 float freq_fric_tune = 1.0f;
Lightvalve 13:747daba9cf59 276
Lightvalve 14:8e7590227d22 277 uint32_t TMR3_COUNT_CAN_TX = 0;
Lightvalve 14:8e7590227d22 278
Lightvalve 57:f4819de54e7a 279 // Current Control Variables
Lightvalve 57:f4819de54e7a 280 double I_REF = 0.0f;
Lightvalve 57:f4819de54e7a 281 double I_REF_fil = 0.0f;
hyhoon 226:82a3ca333004 282 double I_REF_fil_DZ = 0.0f;
Lightvalve 57:f4819de54e7a 283 double I_ERR = 0.0f;
Lightvalve 57:f4819de54e7a 284 double I_ERR_INT = 0.0f;
Lightvalve 57:f4819de54e7a 285 double I_REF_fil_old = 0.0f;
Lightvalve 57:f4819de54e7a 286 double I_REF_fil_diff = 0.0f;
Lightvalve 57:f4819de54e7a 287
Lightvalve 57:f4819de54e7a 288 // system id
Lightvalve 57:f4819de54e7a 289 int cnt_sysid = 0;
Lightvalve 57:f4819de54e7a 290 double freq_sysid_Iref = 0.0f;
Lightvalve 57:f4819de54e7a 291
Lightvalve 169:645207e160ca 292 int cnt_freq_test = 0;
Lightvalve 169:645207e160ca 293 int cnt_step_test = 0;
Lightvalve 169:645207e160ca 294 int buffer_data_size = 0;
Lightvalve 169:645207e160ca 295 int cnt_send_buffer = 0;
Lightvalve 169:645207e160ca 296 float freq_test_valve_ref = 1.0f;
Lightvalve 169:645207e160ca 297 float ref_array[10000];
Lightvalve 169:645207e160ca 298 int pos_array[10000];
Lightvalve 192:637092202815 299 int flag_every_reference = 0;
Lightvalve 169:645207e160ca 300
Lightvalve 14:8e7590227d22 301 int TMR3_COUNT_IREF = 0;
Lightvalve 30:8d561f16383b 302 float CUR_CURRENT = 0.0f;
Lightvalve 30:8d561f16383b 303 float u_CUR[3] = {0.0f,0.0f,0.0f};
Lightvalve 29:69f3f5445d6d 304 int FINDHOME_STAGE = 0;
Lightvalve 29:69f3f5445d6d 305 int FINDHOME_INIT = 0;
Lightvalve 30:8d561f16383b 306 int FINDHOME_GOTOLIMIT = 1;
Lightvalve 30:8d561f16383b 307 int FINDHOME_ZEROPOSE = 2;
Lightvalve 14:8e7590227d22 308
hyhoon 227:699c3e572283 309 int FINDDZ_STAGE = 0;
hyhoon 227:699c3e572283 310 int FINDDZ_INIT = 0;
hyhoon 227:699c3e572283 311 int FINDDZ_START1 = 1;
hyhoon 227:699c3e572283 312 int FINDDZ_START2 = 2;
hyhoon 227:699c3e572283 313 int FINDDZ_STOP = 3;
hyhoon 227:699c3e572283 314
Lightvalve 45:35fa6884d0c6 315 float alpha_trans = 0.0f;
Lightvalve 45:35fa6884d0c6 316
Lightvalve 57:f4819de54e7a 317 float V_out=0.0f;
Lightvalve 57:f4819de54e7a 318 float V_rem=0.0f; // for anti-windup
Lightvalve 57:f4819de54e7a 319 float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
Lightvalve 57:f4819de54e7a 320
Lightvalve 57:f4819de54e7a 321 float PWM_out=0.0f;
Lightvalve 57:f4819de54e7a 322
jobuuu 230:2c3e5ecbe7e1 323 double K_v = 1.0f; // valve flowrate gain 1
jobuuu 230:2c3e5ecbe7e1 324 double C_d = 0.16f; // valve flowrate gain 2
jobuuu 230:2c3e5ecbe7e1 325
Lightvalve 57:f4819de54e7a 326 double mV_PER_mA = 600.0f; // current >> voltage
Lightvalve 57:f4819de54e7a 327 double mV_PER_pulse = 0.6f; // pulse >> voltage
Lightvalve 57:f4819de54e7a 328 double mA_PER_pulse = 0.001f; // pulse >> current
Lightvalve 57:f4819de54e7a 329
Lightvalve 57:f4819de54e7a 330 int timer_while = 0;
Lightvalve 57:f4819de54e7a 331 int while_index = 0;
Lightvalve 170:42c938a40313 332 int RL_timer = 0;
Lightvalve 57:f4819de54e7a 333
Lightvalve 67:c2812cf26c38 334 float K_LPF = 0.0f;
Lightvalve 67:c2812cf26c38 335 float D_LPF = 0.0f;
Lightvalve 67:c2812cf26c38 336
Lightvalve 245:e9c5ec04e378 337 float V_EXI = 0.0f;
Lightvalve 23:59218d4a256d 338
Lightvalve 11:82d8768d7351 339 /*******************************************************************************
Lightvalve 11:82d8768d7351 340 * General math functions
Lightvalve 11:82d8768d7351 341 ******************************************************************************/
Lightvalve 11:82d8768d7351 342
Lightvalve 11:82d8768d7351 343
Lightvalve 34:bb2ca2fc2a8e 344 float dabs(float tx)
Lightvalve 34:bb2ca2fc2a8e 345 {
Lightvalve 30:8d561f16383b 346 if (tx >= 0.0f)
Lightvalve 11:82d8768d7351 347 return tx;
Lightvalve 11:82d8768d7351 348 else
Lightvalve 11:82d8768d7351 349 return -tx;
Lightvalve 11:82d8768d7351 350 }
Lightvalve 11:82d8768d7351 351
Lightvalve 34:bb2ca2fc2a8e 352 float change_int_to_efloat(int input)
Lightvalve 34:bb2ca2fc2a8e 353 {
Lightvalve 11:82d8768d7351 354 int i = 0;
Lightvalve 11:82d8768d7351 355
Lightvalve 30:8d561f16383b 356 float output = 0;
Lightvalve 48:889798ff9329 357 int vn = (int) ((float) input / 10.0f);
Lightvalve 11:82d8768d7351 358 int en = input % 10;
Lightvalve 11:82d8768d7351 359
Lightvalve 30:8d561f16383b 360 float temp = 1.;
Lightvalve 11:82d8768d7351 361 for (i = 0; i < en; i++)
Lightvalve 30:8d561f16383b 362 temp *= 0.1f;
Lightvalve 11:82d8768d7351 363
Lightvalve 30:8d561f16383b 364 output = (float) vn*temp;
Lightvalve 11:82d8768d7351 365 return output;
Lightvalve 11:82d8768d7351 366 }
Lightvalve 11:82d8768d7351 367
Lightvalve 34:bb2ca2fc2a8e 368 void make_delay(void)
Lightvalve 34:bb2ca2fc2a8e 369 {
Lightvalve 11:82d8768d7351 370 int i = 0;
Lightvalve 11:82d8768d7351 371
Lightvalve 65:a2d7c63419c2 372 for (i = 0; i < 1000000; i++) {
Lightvalve 11:82d8768d7351 373 ;
Lightvalve 11:82d8768d7351 374 }
Lightvalve 11:82d8768d7351 375 }
Lightvalve 11:82d8768d7351 376
Lightvalve 11:82d8768d7351 377
Lightvalve 11:82d8768d7351 378 /*******************************************************************************
Lightvalve 11:82d8768d7351 379 * ROM functions
Lightvalve 11:82d8768d7351 380 ******************************************************************************/
Lightvalve 11:82d8768d7351 381
Lightvalve 34:bb2ca2fc2a8e 382 void ROM_CALL_DATA(void)
Lightvalve 34:bb2ca2fc2a8e 383 {
Lightvalve 170:42c938a40313 384 BNO = spi_eeprom_read(RID_BNO);
Lightvalve 170:42c938a40313 385 OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
Lightvalve 170:42c938a40313 386 SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
Lightvalve 170:42c938a40313 387 CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
Lightvalve 170:42c938a40313 388 FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
Lightvalve 170:42c938a40313 389 CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
Lightvalve 170:42c938a40313 390 DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
Lightvalve 170:42c938a40313 391 DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
Lightvalve 170:42c938a40313 392 DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
Lightvalve 170:42c938a40313 393 SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
Lightvalve 170:42c938a40313 394 VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
Lightvalve 170:42c938a40313 395 P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 396 I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 397 D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 398 P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 399 I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 400 D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 401 P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 402 I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 403 D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
hyhoon 226:82a3ca333004 404 VALVE_DEADZONE_PLUS = (spi_eeprom_read(RID_VALVE_DEADZONE_PLUS));
hyhoon 226:82a3ca333004 405 VALVE_DEADZONE_MINUS = (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS));
Lightvalve 170:42c938a40313 406 VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
Lightvalve 238:4e660aa77eb7 407 // COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
Lightvalve 170:42c938a40313 408 VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
Lightvalve 170:42c938a40313 409 VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
Lightvalve 170:42c938a40313 410 BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
Lightvalve 170:42c938a40313 411 CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
Lightvalve 170:42c938a40313 412 CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
Lightvalve 170:42c938a40313 413 PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
Lightvalve 170:42c938a40313 414 PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
Lightvalve 170:42c938a40313 415 PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
Lightvalve 170:42c938a40313 416 alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
jobuuu 230:2c3e5ecbe7e1 417 PRES_SUPPLY_NOM = spi_eeprom_read(RID_PRES_SUPPLY);
jobuuu 230:2c3e5ecbe7e1 418 PRES_SUPPLY = PRES_SUPPLY_NOM;
Lightvalve 170:42c938a40313 419 ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
Lightvalve 170:42c938a40313 420 ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
Lightvalve 170:42c938a40313 421 STROKE = spi_eeprom_read(RID_STROKE);
Lightvalve 180:02be1711ee0b 422 ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
Lightvalve 241:fb5b57e3f157 423 TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.001f;
Lightvalve 170:42c938a40313 424 PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
Lightvalve 170:42c938a40313 425 PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
Lightvalve 170:42c938a40313 426 FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
Lightvalve 170:42c938a40313 427 HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
Lightvalve 170:42c938a40313 428 HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
jobuuu 230:2c3e5ecbe7e1 429 FORCE_VREF = (float) (spi_eeprom_read(RID_FORCE_SENSOR_VREF)) *0.001f;
Lightvalve 170:42c938a40313 430 PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
Lightvalve 170:42c938a40313 431 PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
Lightvalve 170:42c938a40313 432 VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
Lightvalve 170:42c938a40313 433 VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
Lightvalve 170:42c938a40313 434 VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
Lightvalve 170:42c938a40313 435 VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
Lightvalve 170:42c938a40313 436 VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
Lightvalve 170:42c938a40313 437 VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
Lightvalve 170:42c938a40313 438 VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
Lightvalve 170:42c938a40313 439 VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
Lightvalve 170:42c938a40313 440 VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
Lightvalve 170:42c938a40313 441 VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
Lightvalve 170:42c938a40313 442 for(int i=0; i<25; i++) {
Lightvalve 170:42c938a40313 443 VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
Lightvalve 170:42c938a40313 444 }
Lightvalve 170:42c938a40313 445 for(int i=0; i<100; i++) {
Lightvalve 170:42c938a40313 446 JOINT_VEL[i] = ( ((spi_eeprom_read( RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
Lightvalve 170:42c938a40313 447 }
Lightvalve 170:42c938a40313 448 VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
Lightvalve 170:42c938a40313 449 VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
Lightvalve 170:42c938a40313 450 VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
Lightvalve 237:c2cfe349f5c1 451 VALVE_ELECTRIC_CENTER = spi_eeprom_read(RID_VALVE_ELECTRIC_CENTER);
Lightvalve 177:8e9cf31d63f4 452
Lightvalve 224:985dba42f261 453 // K_SPRING = spi_eeprom_read(RID_K_SPRING);
Lightvalve 224:985dba42f261 454 // D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
Lightvalve 177:8e9cf31d63f4 455
Lightvalve 11:82d8768d7351 456 }
Lightvalve 11:82d8768d7351 457
Lightvalve 11:82d8768d7351 458 /*******************************************************************************
Lightvalve 11:82d8768d7351 459 * ENCODER functions
jobuuu 230:2c3e5ecbe7e1 460
Lightvalve 11:82d8768d7351 461 ******************************************************************************/
jobuuu 230:2c3e5ecbe7e1 462 long ENC_pulse = 0, ENC_pulse_old = 0, ENC_pulse_diff = 0;
jobuuu 230:2c3e5ecbe7e1 463 long ENC_pulse_offset = 0;
Lightvalve 11:82d8768d7351 464
Lightvalve 34:bb2ca2fc2a8e 465 void ENC_UPDATE(void)
Lightvalve 34:bb2ca2fc2a8e 466 {
jobuuu 230:2c3e5ecbe7e1 467 ENC_pulse = spi_enc_read(); // Unit : pulse
jobuuu 230:2c3e5ecbe7e1 468 ENC_pulse_diff = ENC_pulse - ENC_pulse_old;
Lightvalve 177:8e9cf31d63f4 469
jobuuu 230:2c3e5ecbe7e1 470 pos.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse + ENC_pulse_offset)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg or mm
jobuuu 230:2c3e5ecbe7e1 471 vel.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse_diff * (long)FREQ_10k)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg/s or mm/s
Lightvalve 177:8e9cf31d63f4 472
jobuuu 230:2c3e5ecbe7e1 473 ENC_pulse_old = ENC_pulse;
Lightvalve 11:82d8768d7351 474 }
Lightvalve 11:82d8768d7351 475
Lightvalve 34:bb2ca2fc2a8e 476 void ENC_SET_ZERO(void)
Lightvalve 34:bb2ca2fc2a8e 477 {
Lightvalve 11:82d8768d7351 478 spi_enc_set_clear();
jobuuu 230:2c3e5ecbe7e1 479 pos.Reset();
jobuuu 230:2c3e5ecbe7e1 480 ENC_pulse_offset = 0;
jobuuu 230:2c3e5ecbe7e1 481 ENC_pulse = ENC_pulse_old = ENC_pulse_diff = 0;
Lightvalve 11:82d8768d7351 482 }
Lightvalve 11:82d8768d7351 483
jobuuu 230:2c3e5ecbe7e1 484 void ENC_SET(long value_e)
Lightvalve 34:bb2ca2fc2a8e 485 {
Lightvalve 11:82d8768d7351 486 spi_enc_set_clear();
jobuuu 230:2c3e5ecbe7e1 487 ENC_pulse_offset = value_e;
jobuuu 230:2c3e5ecbe7e1 488 pos.Reset();
jobuuu 230:2c3e5ecbe7e1 489 pos.sen = value_e/ENC_PULSE_PER_POSITION;
jobuuu 230:2c3e5ecbe7e1 490 ENC_pulse = ENC_pulse_old = value_e;
jobuuu 230:2c3e5ecbe7e1 491 ENC_pulse_diff = 0;
Lightvalve 11:82d8768d7351 492 }
Lightvalve 18:b8adf1582ea3 493
Lightvalve 19:23b7c1ad8683 494
Lightvalve 30:8d561f16383b 495
Lightvalve 57:f4819de54e7a 496