Yeseong Jeong / Mbed 2 deprecated HydraulicControlBoard_Start

20210203

Dependencies:   mbed FastPWM

function_utilities/function_utilities.cpp@213:3e7bee70a0b4, 2020-12-23 (annotated)

Committer:
Lightvalve
Date:
Wed Dec 23 15:20:09 2020 +0000
Revision:
213:3e7bee70a0b4
Parent:
207:c70c5a9f17dd 
201223-5

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 72:3436ce769b1e 39 float I_GAIN_JOINT_TORQUE = 0.0f;
Lightvalve 72:3436ce769b1e 40 float D_GAIN_JOINT_TORQUE = 0.0f;
Lightvalve 72:3436ce769b1e 41 float P_GAIN_JOINT_TORQUE_FF = 0.0f;
Lightvalve 72:3436ce769b1e 42 float I_GAIN_JOINT_TORQUE_FF = 0.0f;
Lightvalve 72:3436ce769b1e 43 float D_GAIN_JOINT_TORQUE_FF = 0.0f;
Lightvalve 11:82d8768d7351 44
Lightvalve 46:2694daea349b 45 int16_t K_SPRING = 0.0;
Lightvalve 46:2694daea349b 46 int16_t D_DAMPER = 0.0;
Lightvalve 72:3436ce769b1e 47
Lightvalve 54:647072f5307a 48 int16_t flag_delay_test = 0;
Lightvalve 46:2694daea349b 49
Lightvalve 36:a46e63505ed8 50 //float P_GAIN_VALVE_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 51 //float I_GAIN_VALVE_POSITION_OPP= 0.0f;
Lightvalve 36:a46e63505ed8 52 //float D_GAIN_VALVE_POSITION_OPP= 0.0f;
Lightvalve 36:a46e63505ed8 53 //float P_GAIN_JOINT_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 54 //float I_GAIN_JOINT_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 55 //float D_GAIN_JOINT_POSITION_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 56 //float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
Lightvalve 36:a46e63505ed8 57 //float I_GAIN_JOINT_TORQUE_OPP = 0.0;
Lightvalve 36:a46e63505ed8 58 //float D_GAIN_JOINT_TORQUE_OPP = 0.0;
Lightvalve 36:a46e63505ed8 59
Lightvalve 33:91b17819ec30 60 float VALVE_DEADZONE_PLUS;
Lightvalve 33:91b17819ec30 61 float VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 62
Lightvalve 11:82d8768d7351 63 int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 64 int16_t COMPLIANCE_GAIN;
Lightvalve 11:82d8768d7351 65
Lightvalve 11:82d8768d7351 66 int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 67
Lightvalve 11:82d8768d7351 68 int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 69
Lightvalve 11:82d8768d7351 70 int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 71
Lightvalve 11:82d8768d7351 72 int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 73 int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 74
Lightvalve 11:82d8768d7351 75 int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 76 int16_t PISTON_AREA_B;
Lightvalve 30:8d561f16383b 77 float PISTON_AREA_alpha;
Lightvalve 57:f4819de54e7a 78 float alpha3 = 1.0f;
Lightvalve 11:82d8768d7351 79
Lightvalve 11:82d8768d7351 80
Lightvalve 11:82d8768d7351 81 int16_t PRES_SUPPLY;
Lightvalve 11:82d8768d7351 82 int16_t PRES_RETURN;
Lightvalve 11:82d8768d7351 83
Lightvalve 11:82d8768d7351 84 int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 85 int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 86
Lightvalve 11:82d8768d7351 87 int16_t STROKE;
Lightvalve 11:82d8768d7351 88
Lightvalve 133:22ab22818e01 89 float Amm = 236.4f;
Lightvalve 133:22ab22818e01 90 float beta = 1300000000.0f;
Lightvalve 133:22ab22818e01 91 float Ps = 10000000.0f; //100bar = 100*10^5 Pa
Lightvalve 133:22ab22818e01 92 float Pt = 0.0f; // 0bar = 0Pa
Lightvalve 133:22ab22818e01 93 //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 94 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 170:42c938a40313 95 float V_adapt = 0.0000053f; // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
Lightvalve 133:22ab22818e01 96 float x_4_des_old = 0.0f;
Lightvalve 213:3e7bee70a0b4 97 float a_hat = -10000000.0f;
Lightvalve 11:82d8768d7351 98
Lightvalve 57:f4819de54e7a 99 //int16_t VALVE_LIMIT_PLUS;
Lightvalve 57:f4819de54e7a 100 //int16_t VALVE_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 101
Lightvalve 48:889798ff9329 102 float ENC_PULSE_PER_POSITION;
Lightvalve 48:889798ff9329 103 float TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 30:8d561f16383b 104 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 105 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 11:82d8768d7351 106
Lightvalve 11:82d8768d7351 107 int HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 108 int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 109
Lightvalve 30:8d561f16383b 110 float FRICTION;
Lightvalve 30:8d561f16383b 111 float REF_PERIOD;
Lightvalve 30:8d561f16383b 112 float REF_MAG;
Lightvalve 11:82d8768d7351 113 int REF_NUM;
Lightvalve 11:82d8768d7351 114
Lightvalve 11:82d8768d7351 115
Lightvalve 30:8d561f16383b 116 float DAC_REF;
Lightvalve 30:8d561f16383b 117 float DAC_RESOL;
Lightvalve 11:82d8768d7351 118
Lightvalve 11:82d8768d7351 119 int REF_POSITION;
Lightvalve 11:82d8768d7351 120 int REF_VELOCITY;
Lightvalve 11:82d8768d7351 121 int16_t REF_TORQUE;
Lightvalve 11:82d8768d7351 122 int16_t REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 123 int16_t REF_PWM;
Lightvalve 11:82d8768d7351 124 int16_t REF_VALVE_POSITION;
Lightvalve 14:8e7590227d22 125 int16_t REF_CURRENT;
Lightvalve 11:82d8768d7351 126
Lightvalve 11:82d8768d7351 127 int REF_MOVE_TIME_5k;
Lightvalve 11:82d8768d7351 128 int INIT_REF_PWM;
Lightvalve 11:82d8768d7351 129 int INIT_REF_VALVE_POS;
Lightvalve 11:82d8768d7351 130 int INIT_REF_POS;
Lightvalve 11:82d8768d7351 131 int INIT_REF_VEL;
Lightvalve 11:82d8768d7351 132 int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 133 int INIT_REF_PRES_DIFF;
Lightvalve 14:8e7590227d22 134 int INIT_REF_CURRENT;
Lightvalve 11:82d8768d7351 135
Lightvalve 11:82d8768d7351 136 int CUR_POSITION;
Lightvalve 11:82d8768d7351 137 int CUR_VELOCITY;
Lightvalve 30:8d561f16383b 138 float CUR_TORQUE;
Lightvalve 30:8d561f16383b 139 float CUR_PRES_A;
Lightvalve 30:8d561f16383b 140 float CUR_PRES_B;
Lightvalve 11:82d8768d7351 141 int CUR_VALVE_POSITION;
Lightvalve 11:82d8768d7351 142
Lightvalve 11:82d8768d7351 143 unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 144 unsigned int TMR2_COUNT_LED2;
Lightvalve 54:647072f5307a 145 unsigned int TMR2_COUNT_CAN_TX = 0;
Lightvalve 54:647072f5307a 146 unsigned int TMR3_COUNT_TEST = 0;
Lightvalve 11:82d8768d7351 147
Lightvalve 11:82d8768d7351 148 int num_err;
Lightvalve 11:82d8768d7351 149 int flag_err[8];
Lightvalve 11:82d8768d7351 150 int flag_err_old[8];
Lightvalve 11:82d8768d7351 151 int flag_err_rt;
Lightvalve 11:82d8768d7351 152
Lightvalve 11:82d8768d7351 153 int flag_ref_enable;
Lightvalve 11:82d8768d7351 154
Lightvalve 11:82d8768d7351 155 int flag_data_request[5];
Lightvalve 11:82d8768d7351 156
Lightvalve 45:35fa6884d0c6 157 int MODE_POS_FT_TRANS = 0;
Lightvalve 66:a8e6799dbce3 158 int NN_Control_Flag = 0;
Lightvalve 45:35fa6884d0c6 159
Lightvalve 169:645207e160ca 160 int cnt_buffer = 0;
Lightvalve 169:645207e160ca 161
Lightvalve 57:f4819de54e7a 162 float CUR_CURRENT_mA = 0.0f;
Lightvalve 57:f4819de54e7a 163 float CUR_PRES_A_BAR = 0.0f;
Lightvalve 57:f4819de54e7a 164 float CUR_PRES_B_BAR = 0.0f;
Lightvalve 57:f4819de54e7a 165 float CUR_TORQUE_NM = 0.0f;
Lightvalve 57:f4819de54e7a 166 float CUR_TORQUE_NM_PRESS = 0.0f;
Lightvalve 11:82d8768d7351 167
Lightvalve 57:f4819de54e7a 168 float PRES_A_VREF = 0.0f;
Lightvalve 57:f4819de54e7a 169 float PRES_B_VREF = 0.0f;
Lightvalve 57:f4819de54e7a 170 float TORQUE_VREF = 0.0f;
Lightvalve 11:82d8768d7351 171
Lightvalve 57:f4819de54e7a 172 float VALVE_PWM_RAW_FB = 0.0f;
Lightvalve 57:f4819de54e7a 173 float VALVE_PWM_RAW_FF = 0.0f;
Lightvalve 57:f4819de54e7a 174 float VALVE_PWM_RAW = 0.0f;
Lightvalve 57:f4819de54e7a 175 int VALVE_PWM_VALVE_DZ = 0;
Lightvalve 11:82d8768d7351 176
Lightvalve 30:8d561f16383b 177 float VALVE_GAIN_LPM_PER_V[10];
Lightvalve 30:8d561f16383b 178 float VALVE_POS_VS_PWM[25];
Lightvalve 11:82d8768d7351 179 long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 180
Lightvalve 11:82d8768d7351 181 int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 182 int VALVE_MIN_POS;
Lightvalve 11:82d8768d7351 183 int VALVE_POS_NUM;
Lightvalve 32:4b8c0fedaf2c 184 float VALVE_CENTER_OFFSET;
Lightvalve 33:91b17819ec30 185 float VALVE_DZ_MINUS_OFFSET;
Lightvalve 33:91b17819ec30 186 float VALVE_DZ_PLUS_OFFSET;
Lightvalve 11:82d8768d7351 187
Lightvalve 57:f4819de54e7a 188 int TMR3_COUNT_FINDHOME = 0;
Lightvalve 57:f4819de54e7a 189 int TMR3_COUNT_FLOWRATE = 0;
Lightvalve 57:f4819de54e7a 190 int TMR3_COUNT_DEADZONE = 0;
Lightvalve 57:f4819de54e7a 191 int TMR3_COUNT_PRES_NULL = 0;
Lightvalve 57:f4819de54e7a 192 int TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve 57:f4819de54e7a 193 int TMR3_COUNT_PRES_CALIB = 0;
Lightvalve 57:f4819de54e7a 194 int TMR3_COUNT_REFERENCE = 0;
Lightvalve 57:f4819de54e7a 195 int TMR3_COUNT_JOINT = 0;
Lightvalve 57:f4819de54e7a 196 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
Lightvalve 11:82d8768d7351 197
Lightvalve 57:f4819de54e7a 198 float TUNING_TIME = 600.0f; // sec
Lightvalve 56:6f50d9d3bfee 199
Lightvalve 57:f4819de54e7a 200 float REFERENCE_FREQ = 1.0f;
Lightvalve 57:f4819de54e7a 201 float REFERENCE_MAG = 0.0f;
Lightvalve 11:82d8768d7351 202
Lightvalve 11:82d8768d7351 203 bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 204
Lightvalve 11:82d8768d7351 205 int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 206 enum _JUMP_STATUS {
Lightvalve 11:82d8768d7351 207 JUMP_NO_ACT = 0, //0
Lightvalve 11:82d8768d7351 208 JUMP_START, //1
Lightvalve 11:82d8768d7351 209 JUMP_TAKEOFF, //2
Lightvalve 11:82d8768d7351 210 JUMP_FLYING, //3
Lightvalve 11:82d8768d7351 211 JUMP_LANDING, //4
Lightvalve 11:82d8768d7351 212 };
Lightvalve 11:82d8768d7351 213
Lightvalve 30:8d561f16383b 214 float CUR_PRES_DIFF_BAR = 0.0f;
Lightvalve 30:8d561f16383b 215 float CUR_PRES_A_sum = 0.0f;
Lightvalve 30:8d561f16383b 216 float CUR_PRES_B_sum = 0.0f;
Lightvalve 30:8d561f16383b 217 float CUR_PRES_A_mean = 0.0f;
Lightvalve 30:8d561f16383b 218 float CUR_PRES_B_mean = 0.0f;
Lightvalve 30:8d561f16383b 219 float CUR_TORQUE_sum = 0.0f;
Lightvalve 30:8d561f16383b 220 float CUR_TORQUE_mean = 0.0f;
Lightvalve 58:2eade98630e2 221 float PRES_A_NULL = 300.0f;
Lightvalve 58:2eade98630e2 222 float PRES_B_NULL = 300.0f;
Lightvalve 30:8d561f16383b 223 float TORQUE_NULL = 3900.0f;
Lightvalve 11:82d8768d7351 224
Lightvalve 30:8d561f16383b 225 float Ref_Valve_Pos_Old = 0.0f;
Lightvalve 11:82d8768d7351 226
Lightvalve 13:747daba9cf59 227 int VALVE_ID_timer = 0;
Lightvalve 13:747daba9cf59 228 int VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 229 int VALVE_FR_timer = 0;
Lightvalve 35:34ce7b0347b8 230 //int VALVE_HPL_timer = 0;
Lightvalve 13:747daba9cf59 231 int VALVE_POS_TMP = 0;
Lightvalve 13:747daba9cf59 232 int JOINT_VEL_TMP = 0;
Lightvalve 13:747daba9cf59 233 int DDV_POS_AVG = 0;
Lightvalve 19:23b7c1ad8683 234 int VALVE_POS_AVG[50] = {0};
Lightvalve 13:747daba9cf59 235 int VALVE_POS_AVG_OLD = 0;
Lightvalve 13:747daba9cf59 236 int data_num = 0;
Lightvalve 13:747daba9cf59 237 int ID_index = 0;
Lightvalve 13:747daba9cf59 238 int DZ_index = 1;
Lightvalve 19:23b7c1ad8683 239 int ID_index_array[50] = {0};
Lightvalve 13:747daba9cf59 240 int first_check = 0;
Lightvalve 30:8d561f16383b 241 float init_time = 0.0f;
Lightvalve 13:747daba9cf59 242 int DZ_case = 0;
Lightvalve 13:747daba9cf59 243 int START_POS = 0;
Lightvalve 13:747daba9cf59 244 int FINAL_POS = 0;
Lightvalve 13:747daba9cf59 245 int DZ_DIRECTION = 0;
Lightvalve 13:747daba9cf59 246 int FIRST_DZ = 0;
Lightvalve 13:747daba9cf59 247 int SECOND_DZ = 0;
Lightvalve 13:747daba9cf59 248 int DZ_NUM = 0;
Lightvalve 13:747daba9cf59 249 int one_period_end = 0;
Lightvalve 30:8d561f16383b 250 float Ref_Vel_Test = 0.0f;
Lightvalve 13:747daba9cf59 251 long TMR2_FOR_SLOW_LOGGING = 0;
Lightvalve 19:23b7c1ad8683 252 //int velcount = 0;
Lightvalve 13:747daba9cf59 253 char max_check = 0;
Lightvalve 34:bb2ca2fc2a8e 254 char min_check = 0;
Lightvalve 13:747daba9cf59 255
Lightvalve 30:8d561f16383b 256 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 257 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 258 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 259 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
Lightvalve 13:747daba9cf59 260
Lightvalve 30:8d561f16383b 261 float CUR_FLOWRATE = 0.0f;
Lightvalve 30:8d561f16383b 262 float VALVE_FF_VOLTAGE = 0.0f;
Lightvalve 13:747daba9cf59 263
Lightvalve 13:747daba9cf59 264 int pos_plus_end = 0;
Lightvalve 13:747daba9cf59 265 int pos_minus_end = 0;
Lightvalve 13:747daba9cf59 266
Lightvalve 13:747daba9cf59 267 bool need_enc_init = false;
Lightvalve 13:747daba9cf59 268
Lightvalve 13:747daba9cf59 269 int temp_time = 0;
Lightvalve 13:747daba9cf59 270
Lightvalve 30:8d561f16383b 271 float CUR_VELOCITY_sum = 0.0f;
Lightvalve 30:8d561f16383b 272 float temp_vel_sum = 0.0f;
Lightvalve 13:747daba9cf59 273
Lightvalve 13:747daba9cf59 274 int DZ_dir = 0;
Lightvalve 13:747daba9cf59 275 int DZ_temp_cnt = 0;
Lightvalve 13:747daba9cf59 276 int DZ_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 277 int DZ_end = 2;
Lightvalve 13:747daba9cf59 278 int flag_flowrate = 0;
Lightvalve 13:747daba9cf59 279 int fl_temp_cnt = 0;
Lightvalve 13:747daba9cf59 280 int fl_temp_cnt2 = 0;
Lightvalve 13:747daba9cf59 281 int cur_vel_sum = 0;
Lightvalve 13:747daba9cf59 282
Lightvalve 57:f4819de54e7a 283 float Cur_Valve_Open_pulse = 0.0f;
Lightvalve 57:f4819de54e7a 284
Lightvalve 13:747daba9cf59 285 // find home
Lightvalve 13:747daba9cf59 286 int CUR_VELOCITY_OLD = 0;
Lightvalve 13:747daba9cf59 287 int cnt_findhome = 0;
Lightvalve 13:747daba9cf59 288 int cnt_vel_findhome = 0;
Lightvalve 13:747daba9cf59 289 int FINDHOME_VELOCITY = 0;
Lightvalve 13:747daba9cf59 290 int FINDHOME_VELOCITY_OLD = 0;
Lightvalve 13:747daba9cf59 291 int FINDHOME_POSITION = 0;
Lightvalve 13:747daba9cf59 292 int FINDHOME_POSITION_OLD = 0;
Lightvalve 13:747daba9cf59 293
Lightvalve 13:747daba9cf59 294 // valve gain
Lightvalve 13:747daba9cf59 295 int check_vel_pos_init = 0;
Lightvalve 13:747daba9cf59 296 int check_vel_pos_fin = 0;
Lightvalve 13:747daba9cf59 297 int check_vel_pos_interv = 0;
Lightvalve 13:747daba9cf59 298 int valve_gain_repeat_cnt = 0;
Lightvalve 30:8d561f16383b 299 float VALVE_VOLTAGE = 0.0f;
Lightvalve 13:747daba9cf59 300
Lightvalve 30:8d561f16383b 301 float freq_fric_tune = 1.0f;
Lightvalve 13:747daba9cf59 302
Lightvalve 14:8e7590227d22 303 uint32_t TMR3_COUNT_CAN_TX = 0;
Lightvalve 14:8e7590227d22 304
Lightvalve 57:f4819de54e7a 305 // Current Control Variables
Lightvalve 57:f4819de54e7a 306 double I_REF = 0.0f;
Lightvalve 57:f4819de54e7a 307 double I_REF_fil = 0.0f;
Lightvalve 57:f4819de54e7a 308 double I_ERR = 0.0f;
Lightvalve 57:f4819de54e7a 309 double I_ERR_INT = 0.0f;
Lightvalve 57:f4819de54e7a 310 double I_REF_fil_old = 0.0f;
Lightvalve 57:f4819de54e7a 311 double I_REF_fil_diff = 0.0f;
Lightvalve 57:f4819de54e7a 312
Lightvalve 57:f4819de54e7a 313 // system id
Lightvalve 57:f4819de54e7a 314 int cnt_sysid = 0;
Lightvalve 57:f4819de54e7a 315 double freq_sysid_Iref = 0.0f;
Lightvalve 57:f4819de54e7a 316
Lightvalve 169:645207e160ca 317 int cnt_freq_test = 0;
Lightvalve 169:645207e160ca 318 int cnt_step_test = 0;
Lightvalve 169:645207e160ca 319 int buffer_data_size = 0;
Lightvalve 169:645207e160ca 320 int cnt_send_buffer = 0;
Lightvalve 169:645207e160ca 321 float freq_test_valve_ref = 1.0f;
Lightvalve 169:645207e160ca 322 float ref_array[10000];
Lightvalve 169:645207e160ca 323 int pos_array[10000];
Lightvalve 192:637092202815 324 int flag_every_reference = 0;
Lightvalve 169:645207e160ca 325
Lightvalve 14:8e7590227d22 326 int TMR3_COUNT_IREF = 0;
Lightvalve 30:8d561f16383b 327 float CUR_CURRENT = 0.0f;
Lightvalve 30:8d561f16383b 328 float u_CUR[3] = {0.0f,0.0f,0.0f};
Lightvalve 29:69f3f5445d6d 329 int FINDHOME_STAGE = 0;
Lightvalve 29:69f3f5445d6d 330 int FINDHOME_INIT = 0;
Lightvalve 30:8d561f16383b 331 int FINDHOME_GOTOLIMIT = 1;
Lightvalve 30:8d561f16383b 332 int FINDHOME_ZEROPOSE = 2;
Lightvalve 14:8e7590227d22 333
Lightvalve 45:35fa6884d0c6 334 float alpha_trans = 0.0f;
Lightvalve 45:35fa6884d0c6 335
Lightvalve 57:f4819de54e7a 336 float V_out=0.0f;
Lightvalve 57:f4819de54e7a 337 float V_rem=0.0f; // for anti-windup
Lightvalve 57:f4819de54e7a 338 float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
Lightvalve 57:f4819de54e7a 339
Lightvalve 57:f4819de54e7a 340 float PWM_out=0.0f;
Lightvalve 57:f4819de54e7a 341
Lightvalve 57:f4819de54e7a 342 double K_v = 0.0f; // valve flowrate gain
Lightvalve 57:f4819de54e7a 343 double mV_PER_mA = 600.0f; // current >> voltage
Lightvalve 57:f4819de54e7a 344 double mV_PER_pulse = 0.6f; // pulse >> voltage
Lightvalve 57:f4819de54e7a 345 double mA_PER_pulse = 0.001f; // pulse >> current
Lightvalve 57:f4819de54e7a 346
Lightvalve 57:f4819de54e7a 347 int timer_while = 0;
Lightvalve 57:f4819de54e7a 348 int while_index = 0;
Lightvalve 170:42c938a40313 349 int RL_timer = 0;
Lightvalve 57:f4819de54e7a 350
Lightvalve 67:c2812cf26c38 351 float K_LPF = 0.0f;
Lightvalve 67:c2812cf26c38 352 float D_LPF = 0.0f;
Lightvalve 67:c2812cf26c38 353
Lightvalve 207:c70c5a9f17dd 354 float totq_sen_past = 0.0f;
Lightvalve 72:3436ce769b1e 355 float torq_ref_past = 0.0f;
Lightvalve 99:7bbcb3c0fb06 356 float output_normalized = 0.0f;
Lightvalve 99:7bbcb3c0fb06 357
Lightvalve 170:42c938a40313 358 int batch = 0;
Lightvalve 170:42c938a40313 359 float train_set_x[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 360 float train_set_error[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 361 float train_set_count[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 362 float state_array[batch_size][num_input_RL] = {0.0f};
Lightvalve 170:42c938a40313 363 float V[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 364 float r[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 365 float td_target[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 366 float delta[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 367 float advantage[batch_size] = {0.0f};
Lightvalve 173:68c7914679ec 368 float return_G[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 369 float mean = 0.0f;
Lightvalve 170:42c938a40313 370 float deviation = 0.0f;
Lightvalve 170:42c938a40313 371 float mean_old = 0.0f;
Lightvalve 170:42c938a40313 372 float deviation_old = 0.0f;
Lightvalve 170:42c938a40313 373 float mean_before_SP = 0.0f;
Lightvalve 170:42c938a40313 374 float deviation_before_SP = 0.0f;
Lightvalve 170:42c938a40313 375 float mean_before_SP_array[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 376 float deviation_before_SP_array[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 377 float mean_array[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 378 float mean_array_old[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 379 float deviation_array[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 380 float deviation_array_old[batch_size] = {0.0f};
Lightvalve 173:68c7914679ec 381
Lightvalve 173:68c7914679ec 382 float hx_c_sum[num_hidden_unit1] = {0.0f};
Lightvalve 173:68c7914679ec 383 float hx_c_sum_array[batch_size][num_hidden_unit1] = {0.0f};
Lightvalve 173:68c7914679ec 384 float hxh_c_sum[num_hidden_unit2] = {0.0f};
Lightvalve 173:68c7914679ec 385 float hxh_c_sum_array[batch_size][num_hidden_unit2] = {0.0f};
Lightvalve 173:68c7914679ec 386 float hxhh_c_sum = 0.0f;
Lightvalve 173:68c7914679ec 387 float hxhh_c_sum_array[batch_size] = {0.0f};
Lightvalve 173:68c7914679ec 388
Lightvalve 173:68c7914679ec 389 float hx_a_sum[num_hidden_unit1] = {0.0f};
Lightvalve 173:68c7914679ec 390 float hx_a_sum_array[batch_size][num_hidden_unit1] = {0.0f};
Lightvalve 173:68c7914679ec 391 float hxh_a_sum[num_hidden_unit2] = {0.0f};
Lightvalve 173:68c7914679ec 392 float hxh_a_sum_array[batch_size][num_hidden_unit2] = {0.0f};
Lightvalve 173:68c7914679ec 393 float hxhh_a_sum[2] = {0.0f};
Lightvalve 173:68c7914679ec 394 float hxhh_a_sum_array[batch_size][2] = {0.0f};
Lightvalve 173:68c7914679ec 395
Lightvalve 170:42c938a40313 396 float action = 0.0f;
Lightvalve 170:42c938a40313 397 float action_array[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 398 float ratio[batch_size] = {1.0f};
Lightvalve 170:42c938a40313 399 float pi[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 400 float pi_old[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 401 float epsilon = 0.2f;
Lightvalve 170:42c938a40313 402 float surr1[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 403 float surr2[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 404 float loss[batch_size] = {0.0f};
Lightvalve 170:42c938a40313 405 float loss_batch = 0.0f;
Lightvalve 179:d5377766d7ea 406 float gamma = 0.996f;
Lightvalve 170:42c938a40313 407 float lmbda = 0.95f;
Lightvalve 170:42c938a40313 408 char Update_Done_Flag = 1;
Lightvalve 170:42c938a40313 409 char Update_Case = 0;
Lightvalve 170:42c938a40313 410 float reward_sum = 0.0f;
Lightvalve 72:3436ce769b1e 411
Lightvalve 170:42c938a40313 412 float virt_pos = 0.0f;
Lightvalve 170:42c938a40313 413 float logging1 = 0.0f;
Lightvalve 170:42c938a40313 414 float logging2 = 0.0f;
Lightvalve 170:42c938a40313 415 float logging3 = 0.0f;
Lightvalve 170:42c938a40313 416 float logging4 = 0.0f;
Lightvalve 170:42c938a40313 417 float logging5 = 0.0f;
Lightvalve 170:42c938a40313 418
Lightvalve 23:59218d4a256d 419
Lightvalve 11:82d8768d7351 420 /*******************************************************************************
Lightvalve 11:82d8768d7351 421 * General math functions
Lightvalve 11:82d8768d7351 422 ******************************************************************************/
Lightvalve 11:82d8768d7351 423
Lightvalve 11:82d8768d7351 424
Lightvalve 34:bb2ca2fc2a8e 425 float dabs(float tx)
Lightvalve 34:bb2ca2fc2a8e 426 {
Lightvalve 30:8d561f16383b 427 if (tx >= 0.0f)
Lightvalve 11:82d8768d7351 428 return tx;
Lightvalve 11:82d8768d7351 429 else
Lightvalve 11:82d8768d7351 430 return -tx;
Lightvalve 11:82d8768d7351 431 }
Lightvalve 11:82d8768d7351 432
Lightvalve 34:bb2ca2fc2a8e 433 float change_int_to_efloat(int input)
Lightvalve 34:bb2ca2fc2a8e 434 {
Lightvalve 11:82d8768d7351 435 int i = 0;
Lightvalve 11:82d8768d7351 436
Lightvalve 30:8d561f16383b 437 float output = 0;
Lightvalve 48:889798ff9329 438 int vn = (int) ((float) input / 10.0f);
Lightvalve 11:82d8768d7351 439 int en = input % 10;
Lightvalve 11:82d8768d7351 440
Lightvalve 30:8d561f16383b 441 float temp = 1.;
Lightvalve 11:82d8768d7351 442 for (i = 0; i < en; i++)
Lightvalve 30:8d561f16383b 443 temp *= 0.1f;
Lightvalve 11:82d8768d7351 444
Lightvalve 30:8d561f16383b 445 output = (float) vn*temp;
Lightvalve 11:82d8768d7351 446 return output;
Lightvalve 11:82d8768d7351 447 }
Lightvalve 11:82d8768d7351 448
Lightvalve 34:bb2ca2fc2a8e 449 void make_delay(void)
Lightvalve 34:bb2ca2fc2a8e 450 {
Lightvalve 11:82d8768d7351 451 int i = 0;
Lightvalve 11:82d8768d7351 452
Lightvalve 65:a2d7c63419c2 453 for (i = 0; i < 1000000; i++) {
Lightvalve 11:82d8768d7351 454 ;
Lightvalve 11:82d8768d7351 455 }
Lightvalve 11:82d8768d7351 456
Lightvalve 11:82d8768d7351 457 }
Lightvalve 11:82d8768d7351 458
Lightvalve 11:82d8768d7351 459
Lightvalve 11:82d8768d7351 460 /*******************************************************************************
Lightvalve 11:82d8768d7351 461 * ROM functions
Lightvalve 11:82d8768d7351 462 ******************************************************************************/
Lightvalve 34:bb2ca2fc2a8e 463 void ROM_RESET_DATA(void)
Lightvalve 34:bb2ca2fc2a8e 464 {
Lightvalve 16:903b5a4433b4 465 FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
Lightvalve 16:903b5a4433b4 466 if (!writer.ready()) writer.open();
Lightvalve 16:903b5a4433b4 467 writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀
Lightvalve 16:903b5a4433b4 468 writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE);
Lightvalve 52:8ea76864368a 469 writer.write(RID_SENSING_MODE, (int) SENSING_MODE);
Lightvalve 52:8ea76864368a 470 writer.write(RID_CURRENT_CONTROL_MODE, (int) CURRENT_CONTROL_MODE);
Lightvalve 52:8ea76864368a 471 writer.write(RID_FLAG_VALVE_DEADZONE, (int) FLAG_VALVE_DEADZONE);
Lightvalve 16:903b5a4433b4 472 writer.write(RID_CAN_FREQ,(int) CAN_FREQ);
Lightvalve 16:903b5a4433b4 473 writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC);
Lightvalve 16:903b5a4433b4 474 writer.write(RID_VALVE_DIR,(int) DIR_VALVE);
Lightvalve 16:903b5a4433b4 475 writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC);
Lightvalve 30:8d561f16383b 476 writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0f));
Lightvalve 30:8d561f16383b 477 writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0f));
Lightvalve 16:903b5a4433b4 478 writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 479 writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 480 writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 481 writer.write(RID_P_GAIN_JOINT_POSITION,(int) P_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 482 writer.write(RID_I_GAIN_JOINT_POSITION,(int) I_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 483 writer.write(RID_D_GAIN_JOINT_POSITION,(int) D_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 484 writer.write(RID_P_GAIN_JOINT_TORQUE,(int) P_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 485 writer.write(RID_I_GAIN_JOINT_TORQUE,(int) I_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 486 writer.write(RID_D_GAIN_JOINT_TORQUE,(int) D_GAIN_JOINT_TORQUE);
Lightvalve 33:91b17819ec30 487 writer.write(RID_VALVE_DEADZONE_PLUS,(int) (VALVE_DEADZONE_PLUS * 10.0f));
Lightvalve 33:91b17819ec30 488 writer.write(RID_VALVE_DEADZONE_MINUS,(int) (VALVE_DEADZONE_MINUS * 10.0f));
Lightvalve 16:903b5a4433b4 489 writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN);
Lightvalve 16:903b5a4433b4 490 writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN);
Lightvalve 16:903b5a4433b4 491 writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER);
Lightvalve 16:903b5a4433b4 492 writer.write(RID_VALVE_FF,(int) VALVE_FF);
Lightvalve 16:903b5a4433b4 493 writer.write(RID_BULK_MODULUS,(int) BNO);
Lightvalve 16:903b5a4433b4 494 writer.write(RID_CHAMBER_VOLUME_A,(int) CHAMBER_VOLUME_A);
Lightvalve 16:903b5a4433b4 495 writer.write(RID_CHAMBER_VOLUME_B,(int) CHAMBER_VOLUME_B);
Lightvalve 16:903b5a4433b4 496 writer.write(RID_PISTON_AREA_A,(int) PISTON_AREA_A);
Lightvalve 16:903b5a4433b4 497 writer.write(RID_PISTON_AREA_B,(int) PISTON_AREA_B);
Lightvalve 16:903b5a4433b4 498 writer.write(RID_PRES_SUPPLY,(int) PRES_SUPPLY);
Lightvalve 16:903b5a4433b4 499 writer.write(RID_PRES_RETURN,(int) PRES_RETURN);
Lightvalve 16:903b5a4433b4 500 writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS);
Lightvalve 16:903b5a4433b4 501 writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS);
Lightvalve 16:903b5a4433b4 502 writer.write(RID_STROKE,(int) STROKE);
Lightvalve 57:f4819de54e7a 503 //writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
Lightvalve 57:f4819de54e7a 504 //writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
Lightvalve 180:02be1711ee0b 505 //writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f));
Lightvalve 180:02be1711ee0b 506 writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION));
Lightvalve 58:2eade98630e2 507 writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f));
Lightvalve 30:8d561f16383b 508 writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
Lightvalve 30:8d561f16383b 509 writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 510 writer.write(RID_FRICTION,(int) (FRICTION * 10.0f));
Lightvalve 34:bb2ca2fc2a8e 511 writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET);
Lightvalve 34:bb2ca2fc2a8e 512 writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING);
Lightvalve 34:bb2ca2fc2a8e 513 writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f));
Lightvalve 30:8d561f16383b 514 writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f));
Lightvalve 30:8d561f16383b 515 writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f));
Lightvalve 34:bb2ca2fc2a8e 516 writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 517 writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 518 writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 519 writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 520 writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 521 writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 522 writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 523 writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 524 writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
Lightvalve 30:8d561f16383b 525 writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
Lightvalve 34:bb2ca2fc2a8e 526 for(int i=0; i<25; i++) {
Lightvalve 18:b8adf1582ea3 527 writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]);
Lightvalve 18:b8adf1582ea3 528 }
Lightvalve 34:bb2ca2fc2a8e 529 for(int i=0; i<100; i++) {
Lightvalve 19:23b7c1ad8683 530 writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF));
Lightvalve 19:23b7c1ad8683 531 writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF));
Lightvalve 18:b8adf1582ea3 532 }
Lightvalve 18:b8adf1582ea3 533 writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS);
Lightvalve 18:b8adf1582ea3 534 writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS);
Lightvalve 57:f4819de54e7a 535 //writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
Lightvalve 18:b8adf1582ea3 536 writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM);
Lightvalve 177:8e9cf31d63f4 537
Lightvalve 46:2694daea349b 538 writer.write(RID_K_SPRING, (int) K_SPRING);
Lightvalve 46:2694daea349b 539 writer.write(RID_D_DAMPER, (int) D_DAMPER);
Lightvalve 11:82d8768d7351 540
Lightvalve 34:bb2ca2fc2a8e 541 writer.close();
Lightvalve 34:bb2ca2fc2a8e 542
Lightvalve 11:82d8768d7351 543 }
Lightvalve 11:82d8768d7351 544
Lightvalve 34:bb2ca2fc2a8e 545 void ROM_CALL_DATA(void)
Lightvalve 34:bb2ca2fc2a8e 546 {
Lightvalve 170:42c938a40313 547 BNO = spi_eeprom_read(RID_BNO);
Lightvalve 180:02be1711ee0b 548 BNO = 1;
Lightvalve 170:42c938a40313 549 OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
Lightvalve 170:42c938a40313 550 SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
Lightvalve 170:42c938a40313 551 // SENSING_MODE = 1;
Lightvalve 170:42c938a40313 552 CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
Lightvalve 170:42c938a40313 553 // CURRENT_CONTROL_MODE = 1;
Lightvalve 170:42c938a40313 554 FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
Lightvalve 170:42c938a40313 555 CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
Lightvalve 170:42c938a40313 556 // CAN_FREQ = 500;
Lightvalve 170:42c938a40313 557 DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
Lightvalve 170:42c938a40313 558 DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
Lightvalve 170:42c938a40313 559 DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
Lightvalve 170:42c938a40313 560 SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
Lightvalve 170:42c938a40313 561 VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
Lightvalve 170:42c938a40313 562 P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 563 I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 564 D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 565 P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 566 I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 567 D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 568 P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 569 I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 570 D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 571 VALVE_DEADZONE_PLUS = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f;
Lightvalve 170:42c938a40313 572 VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f;
Lightvalve 170:42c938a40313 573 VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
Lightvalve 170:42c938a40313 574 COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
Lightvalve 170:42c938a40313 575 VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
Lightvalve 170:42c938a40313 576 VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
Lightvalve 170:42c938a40313 577 BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
Lightvalve 170:42c938a40313 578 CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
Lightvalve 170:42c938a40313 579 CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
Lightvalve 170:42c938a40313 580 PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
Lightvalve 170:42c938a40313 581 PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
Lightvalve 170:42c938a40313 582 PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
Lightvalve 170:42c938a40313 583 alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
Lightvalve 170:42c938a40313 584 PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
Lightvalve 170:42c938a40313 585 PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
Lightvalve 170:42c938a40313 586 ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
Lightvalve 170:42c938a40313 587 ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
Lightvalve 170:42c938a40313 588 STROKE = spi_eeprom_read(RID_STROKE);
Lightvalve 170:42c938a40313 589 //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
Lightvalve 170:42c938a40313 590 //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
Lightvalve 180:02be1711ee0b 591 //ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION)) * 0.1f;
Lightvalve 180:02be1711ee0b 592 ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
Lightvalve 170:42c938a40313 593 TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
Lightvalve 170:42c938a40313 594 //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
Lightvalve 170:42c938a40313 595 // TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
Lightvalve 170:42c938a40313 596 PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
Lightvalve 170:42c938a40313 597 // PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
Lightvalve 170:42c938a40313 598 PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
Lightvalve 170:42c938a40313 599 // PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
Lightvalve 170:42c938a40313 600 FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
Lightvalve 170:42c938a40313 601 HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
Lightvalve 170:42c938a40313 602 HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
Lightvalve 170:42c938a40313 603 TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
Lightvalve 170:42c938a40313 604 PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
Lightvalve 170:42c938a40313 605 PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
Lightvalve 170:42c938a40313 606 VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
Lightvalve 170:42c938a40313 607 VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
Lightvalve 170:42c938a40313 608 VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
Lightvalve 170:42c938a40313 609 VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
Lightvalve 170:42c938a40313 610 VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
Lightvalve 170:42c938a40313 611 VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
Lightvalve 170:42c938a40313 612 VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
Lightvalve 170:42c938a40313 613 VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
Lightvalve 170:42c938a40313 614 VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
Lightvalve 170:42c938a40313 615 VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
Lightvalve 170:42c938a40313 616 for(int i=0; i<25; i++) {
Lightvalve 170:42c938a40313 617 VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
Lightvalve 170:42c938a40313 618 }
Lightvalve 170:42c938a40313 619 for(int i=0; i<100; i++) {
Lightvalve 170:42c938a40313 620 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 621 }
Lightvalve 170:42c938a40313 622 VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
Lightvalve 170:42c938a40313 623 VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
Lightvalve 170:42c938a40313 624 //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
Lightvalve 170:42c938a40313 625 VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
Lightvalve 177:8e9cf31d63f4 626
Lightvalve 170:42c938a40313 627 K_SPRING = spi_eeprom_read(RID_K_SPRING);
Lightvalve 170:42c938a40313 628 D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
Lightvalve 177:8e9cf31d63f4 629
Lightvalve 177:8e9cf31d63f4 630
Lightvalve 177:8e9cf31d63f4 631
Lightvalve 177:8e9cf31d63f4 632
Lightvalve 177:8e9cf31d63f4 633
Lightvalve 170:42c938a40313 634 /*
Lightvalve 177:8e9cf31d63f4 635
Lightvalve 16:903b5a4433b4 636 BNO = flashReadInt(Rom_Sector, RID_BNO);
Lightvalve 177:8e9cf31d63f4 637 // BNO = 1;
Lightvalve 16:903b5a4433b4 638 OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
Lightvalve 177:8e9cf31d63f4 639 // OPERATING_MODE = 5;
Lightvalve 52:8ea76864368a 640 SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
Lightvalve 177:8e9cf31d63f4 641 // SENSING_MODE = 0;
Lightvalve 52:8ea76864368a 642 CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE);
Lightvalve 177:8e9cf31d63f4 643 // CURRENT_CONTROL_MODE = 0;
Lightvalve 52:8ea76864368a 644 FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE);
Lightvalve 16:903b5a4433b4 645 CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
Lightvalve 16:903b5a4433b4 646 DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR);
Lightvalve 16:903b5a4433b4 647 DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
Lightvalve 16:903b5a4433b4 648 DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
Lightvalve 30:8d561f16383b 649 SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
Lightvalve 30:8d561f16383b 650 VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
Lightvalve 16:903b5a4433b4 651 P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 652 I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 653 D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
Lightvalve 16:903b5a4433b4 654 P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 655 I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 656 D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION);
Lightvalve 16:903b5a4433b4 657 P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 658 I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE);
Lightvalve 16:903b5a4433b4 659 D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE);
Lightvalve 33:91b17819ec30 660 VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f;
Lightvalve 33:91b17819ec30 661 VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
Lightvalve 16:903b5a4433b4 662 VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
Lightvalve 16:903b5a4433b4 663 COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
Lightvalve 16:903b5a4433b4 664 VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
Lightvalve 16:903b5a4433b4 665 VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
Lightvalve 16:903b5a4433b4 666 BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);
Lightvalve 16:903b5a4433b4 667 CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A);
Lightvalve 16:903b5a4433b4 668 CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
Lightvalve 16:903b5a4433b4 669 PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
Lightvalve 16:903b5a4433b4 670 PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
Lightvalve 69:3995ffeaa786 671 PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
Lightvalve 57:f4819de54e7a 672 alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
Lightvalve 16:903b5a4433b4 673 PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
Lightvalve 16:903b5a4433b4 674 PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
Lightvalve 16:903b5a4433b4 675 ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
Lightvalve 16:903b5a4433b4 676 ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
Lightvalve 16:903b5a4433b4 677 STROKE = flashReadInt(Rom_Sector, RID_STROKE);
Lightvalve 57:f4819de54e7a 678 //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
Lightvalve 57:f4819de54e7a 679 //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
Lightvalve 48:889798ff9329 680 ENC_PULSE_PER_POSITION = (float) (flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION)) * 0.1f;
Lightvalve 177:8e9cf31d63f4 681 // ENC_PULSE_PER_POSITION = (float) 1024.0f;
Lightvalve 58:2eade98630e2 682 TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
Lightvalve 48:889798ff9329 683 //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
Lightvalve 177:8e9cf31d63f4 684 // TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
Lightvalve 30:8d561f16383b 685 PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
Lightvalve 177:8e9cf31d63f4 686 // PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 687 PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
Lightvalve 177:8e9cf31d63f4 688 // PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
Lightvalve 30:8d561f16383b 689 FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
Lightvalve 16:903b5a4433b4 690 HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
Lightvalve 16:903b5a4433b4 691 HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
Lightvalve 30:8d561f16383b 692 TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
Lightvalve 32:4b8c0fedaf2c 693 PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f;
Lightvalve 32:4b8c0fedaf2c 694 PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f;
Lightvalve 30:8d561f16383b 695 VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
Lightvalve 30:8d561f16383b 696 VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
Lightvalve 30:8d561f16383b 697 VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
Lightvalve 30:8d561f16383b 698 VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
Lightvalve 30:8d561f16383b 699 VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
Lightvalve 30:8d561f16383b 700 VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
Lightvalve 30:8d561f16383b 701 VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
Lightvalve 30:8d561f16383b 702 VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
Lightvalve 30:8d561f16383b 703 VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
Lightvalve 30:8d561f16383b 704 VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
Lightvalve 34:bb2ca2fc2a8e 705 for(int i=0; i<25; i++) {
Lightvalve 30:8d561f16383b 706 VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
Lightvalve 16:903b5a4433b4 707 }
Lightvalve 34:bb2ca2fc2a8e 708 for(int i=0; i<100; i++) {
Lightvalve 16:903b5a4433b4 709 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 710 }
Lightvalve 16:903b5a4433b4 711 VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
Lightvalve 16:903b5a4433b4 712 VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
Lightvalve 57:f4819de54e7a 713 //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
Lightvalve 16:903b5a4433b4 714 VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
Lightvalve 177:8e9cf31d63f4 715
Lightvalve 46:2694daea349b 716 K_SPRING = flashReadInt(Rom_Sector, RID_K_SPRING);
Lightvalve 46:2694daea349b 717 D_DAMPER = flashReadInt(Rom_Sector, RID_D_DAMPER);
Lightvalve 177:8e9cf31d63f4 718
Lightvalve 177:8e9cf31d63f4 719 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 720
Lightvalve 170:42c938a40313 721 */
Lightvalve 177:8e9cf31d63f4 722
Lightvalve 11:82d8768d7351 723 }
Lightvalve 11:82d8768d7351 724
Lightvalve 11:82d8768d7351 725 /*******************************************************************************
Lightvalve 11:82d8768d7351 726 * ENCODER functions
Lightvalve 11:82d8768d7351 727 ******************************************************************************/
Lightvalve 11:82d8768d7351 728 // A-KHA
Lightvalve 30:8d561f16383b 729 #define KF_G1_11 0.083920206005350f
Lightvalve 30:8d561f16383b 730 #define KF_G1_12 0.000013905329560f
Lightvalve 30:8d561f16383b 731 #define KF_G1_21 -0.000575742328210f
Lightvalve 30:8d561f16383b 732 #define KF_G1_22 0.799999939711725f
Lightvalve 11:82d8768d7351 733 // K
Lightvalve 30:8d561f16383b 734 #define KF_G2_11 0.916079793994650f
Lightvalve 30:8d561f16383b 735 #define KF_G2_12 0.000002878711641f
Lightvalve 30:8d561f16383b 736 #define KF_G2_21 0.000575742328210f
Lightvalve 30:8d561f16383b 737 #define KF_G2_22 0.199999945139809f
Lightvalve 11:82d8768d7351 738
Lightvalve 30:8d561f16383b 739 float KF_Y_11 = 0.0f;
Lightvalve 30:8d561f16383b 740 float KF_Y_21 = 0.0f;
Lightvalve 30:8d561f16383b 741 float KF_X_11 = 0.0f;
Lightvalve 30:8d561f16383b 742 float KF_X_21 = 0.0f;
Lightvalve 11:82d8768d7351 743
Lightvalve 11:82d8768d7351 744 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
Lightvalve 11:82d8768d7351 745 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
Lightvalve 11:82d8768d7351 746 long enc_offset = 0;
Lightvalve 11:82d8768d7351 747
Lightvalve 34:bb2ca2fc2a8e 748 void ENC_UPDATE(void)
Lightvalve 34:bb2ca2fc2a8e 749 {
Lightvalve 11:82d8768d7351 750
Lightvalve 11:82d8768d7351 751 ENC_pos_cur = spi_enc_read();
Lightvalve 11:82d8768d7351 752 ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
Lightvalve 11:82d8768d7351 753
Lightvalve 57:f4819de54e7a 754 //Kalman Filter
Lightvalve 57:f4819de54e7a 755 // ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
Lightvalve 54:647072f5307a 756 // KF_Y_11 = ENC_pos_cur;
Lightvalve 54:647072f5307a 757 // KF_Y_21 = ENC_VEL_RAW;
Lightvalve 54:647072f5307a 758 // 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 759 // 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 760 // ENC_VEL_KF = (int32_t) KF_X_21;
Lightvalve 54:647072f5307a 761 //
Lightvalve 54:647072f5307a 762 // pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
Lightvalve 54:647072f5307a 763 // vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
Lightvalve 177:8e9cf31d63f4 764
Lightvalve 57:f4819de54e7a 765 //Low Pass Filter
Lightvalve 177:8e9cf31d63f4 766
Lightvalve 54:647072f5307a 767 double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset);
Lightvalve 57:f4819de54e7a 768 double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k);
Lightvalve 177:8e9cf31d63f4 769
Lightvalve 57:f4819de54e7a 770 double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
Lightvalve 54:647072f5307a 771 pos.sen = NEW_POSITION;
Lightvalve 57:f4819de54e7a 772 vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
Lightvalve 57:f4819de54e7a 773
Lightvalve 11:82d8768d7351 774 ENC_pos_old = ENC_pos_cur;
Lightvalve 11:82d8768d7351 775 }
Lightvalve 11:82d8768d7351 776
Lightvalve 34:bb2ca2fc2a8e 777 void ENC_SET_ZERO(void)
Lightvalve 34:bb2ca2fc2a8e 778 {
Lightvalve 11:82d8768d7351 779
Lightvalve 11:82d8768d7351 780 spi_enc_set_clear();
Lightvalve 11:82d8768d7351 781 CUR_POSITION = 0;
Lightvalve 11:82d8768d7351 782 ENC_pos_old = ENC_pos_cur = 0;
Lightvalve 11:82d8768d7351 783
Lightvalve 11:82d8768d7351 784 }
Lightvalve 11:82d8768d7351 785
Lightvalve 34:bb2ca2fc2a8e 786 void ENC_SET(int32_t value_e)
Lightvalve 34:bb2ca2fc2a8e 787 {
Lightvalve 11:82d8768d7351 788 spi_enc_set_clear();
Lightvalve 13:747daba9cf59 789 enc_offset = value_e;
Lightvalve 13:747daba9cf59 790 CUR_POSITION = value_e;
Lightvalve 13:747daba9cf59 791 ENC_pos_old = ENC_pos_cur = value_e;
Lightvalve 11:82d8768d7351 792
Lightvalve 11:82d8768d7351 793 }
Lightvalve 18:b8adf1582ea3 794
Lightvalve 19:23b7c1ad8683 795
Lightvalve 30:8d561f16383b 796
Lightvalve 57:f4819de54e7a 797