Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_LIGHT

For LIGHT

Dependencies:   mbed FastPWM

function_utilities/function_utilities.cpp@15:bd0d12728506, 2019-08-30 (annotated)

Committer:
Lightvalve
Date:
Fri Aug 30 02:26:11 2019 +0000
Revision:
15:bd0d12728506
Parent:
14:8e7590227d22
Child:
16:903b5a4433b4
190830

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 11:82d8768d7351 5
Lightvalve 11:82d8768d7351 6 /*******************************************************************************
Lightvalve 11:82d8768d7351 7 * VARIABLE
Lightvalve 11:82d8768d7351 8 ******************************************************************************/
Lightvalve 11:82d8768d7351 9
Lightvalve 11:82d8768d7351 10 // Board Information
Lightvalve 11:82d8768d7351 11 uint8_t BNO = 0;
Lightvalve 11:82d8768d7351 12 uint8_t CONTROL_MODE = 0;
Lightvalve 11:82d8768d7351 13 uint8_t OPERATING_MODE = 0;
Lightvalve 11:82d8768d7351 14 uint8_t SETTING_SWITCH = 0;
Lightvalve 11:82d8768d7351 15 uint8_t SETTING_SWITCH_OLD = 0;
Lightvalve 11:82d8768d7351 16 uint8_t REFERENCE_MODE = 0;
Lightvalve 14:8e7590227d22 17 uint16_t CAN_FREQ = 500;
Lightvalve 11:82d8768d7351 18 uint8_t DIR_JOINT_ENC = 0;
Lightvalve 11:82d8768d7351 19 uint8_t DIR_VALVE = 0;
Lightvalve 11:82d8768d7351 20 uint8_t DIR_VALVE_ENC = 0;
Lightvalve 11:82d8768d7351 21
Lightvalve 11:82d8768d7351 22 double SUPPLY_VOLTAGE = 12.0;
Lightvalve 11:82d8768d7351 23 double VALVE_VOLTAGE_LIMIT = 5.0;
Lightvalve 11:82d8768d7351 24
Lightvalve 11:82d8768d7351 25 double P_GAIN_VALVE_POSITION = 0.0;
Lightvalve 11:82d8768d7351 26 double I_GAIN_VALVE_POSITION= 0.0;
Lightvalve 11:82d8768d7351 27 double D_GAIN_VALVE_POSITION= 0.0;
Lightvalve 11:82d8768d7351 28 double P_GAIN_JOINT_POSITION = 0.0;
Lightvalve 11:82d8768d7351 29 double I_GAIN_JOINT_POSITION = 0.0;
Lightvalve 11:82d8768d7351 30 double D_GAIN_JOINT_POSITION = 0.0;
Lightvalve 11:82d8768d7351 31 double P_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 32 double I_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 33 double D_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 34
Lightvalve 11:82d8768d7351 35 int16_t VALVE_DEADZONE_PLUS;
Lightvalve 11:82d8768d7351 36 int16_t VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 37
Lightvalve 11:82d8768d7351 38 int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 39 int16_t COMPLIANCE_GAIN;
Lightvalve 11:82d8768d7351 40
Lightvalve 11:82d8768d7351 41 int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 42
Lightvalve 11:82d8768d7351 43 int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 44
Lightvalve 11:82d8768d7351 45 int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 46
Lightvalve 11:82d8768d7351 47 int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 48 int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 49
Lightvalve 11:82d8768d7351 50 int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 51 int16_t PISTON_AREA_B;
Lightvalve 11:82d8768d7351 52 double PISTON_AREA_alpha;
Lightvalve 11:82d8768d7351 53
Lightvalve 11:82d8768d7351 54
Lightvalve 11:82d8768d7351 55 int16_t PRES_SUPPLY;
Lightvalve 11:82d8768d7351 56 int16_t PRES_RETURN;
Lightvalve 11:82d8768d7351 57
Lightvalve 11:82d8768d7351 58 int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 59 int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 60
Lightvalve 11:82d8768d7351 61 int16_t STROKE;
Lightvalve 11:82d8768d7351 62
Lightvalve 11:82d8768d7351 63
Lightvalve 11:82d8768d7351 64 int16_t VALVE_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 65 int16_t VALVE_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 66
Lightvalve 11:82d8768d7351 67 int16_t ENC_PULSE_PER_POSITION;
Lightvalve 11:82d8768d7351 68 int16_t TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 14:8e7590227d22 69 double PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
Lightvalve 14:8e7590227d22 70 double PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
Lightvalve 11:82d8768d7351 71
Lightvalve 11:82d8768d7351 72 int HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 73 int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 74
Lightvalve 11:82d8768d7351 75 double FRICTION;
Lightvalve 11:82d8768d7351 76 double REF_PERIOD;
Lightvalve 11:82d8768d7351 77 double REF_MAG;
Lightvalve 11:82d8768d7351 78 int REF_NUM;
Lightvalve 11:82d8768d7351 79
Lightvalve 11:82d8768d7351 80
Lightvalve 11:82d8768d7351 81 double DAC_REF;
Lightvalve 11:82d8768d7351 82 double DAC_RESOL;
Lightvalve 11:82d8768d7351 83 double PWM_RESOL;
Lightvalve 11:82d8768d7351 84
Lightvalve 11:82d8768d7351 85 int REF_POSITION;
Lightvalve 11:82d8768d7351 86 int REF_VELOCITY;
Lightvalve 11:82d8768d7351 87 int16_t REF_TORQUE;
Lightvalve 11:82d8768d7351 88 int16_t REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 89 int16_t REF_PWM;
Lightvalve 11:82d8768d7351 90 int16_t REF_VALVE_POSITION;
Lightvalve 14:8e7590227d22 91 int16_t REF_CURRENT;
Lightvalve 11:82d8768d7351 92
Lightvalve 11:82d8768d7351 93 int REF_MOVE_TIME_5k;
Lightvalve 11:82d8768d7351 94 int INIT_REF_PWM;
Lightvalve 11:82d8768d7351 95 int INIT_REF_VALVE_POS;
Lightvalve 11:82d8768d7351 96 int INIT_REF_POS;
Lightvalve 11:82d8768d7351 97 int INIT_REF_VEL;
Lightvalve 11:82d8768d7351 98 int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 99 int INIT_REF_PRES_DIFF;
Lightvalve 14:8e7590227d22 100 int INIT_REF_CURRENT;
Lightvalve 11:82d8768d7351 101
Lightvalve 11:82d8768d7351 102 int CUR_POSITION;
Lightvalve 11:82d8768d7351 103 int CUR_VELOCITY;
Lightvalve 11:82d8768d7351 104 double CUR_TORQUE;
Lightvalve 11:82d8768d7351 105 double CUR_PRES_A;
Lightvalve 11:82d8768d7351 106 double CUR_PRES_B;
Lightvalve 11:82d8768d7351 107 int CUR_PWM;
Lightvalve 11:82d8768d7351 108 int CUR_VALVE_POSITION;
Lightvalve 11:82d8768d7351 109
Lightvalve 11:82d8768d7351 110 unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 111 unsigned int TMR2_COUNT_LED2;
Lightvalve 11:82d8768d7351 112 unsigned int TMR2_COUNT_CAN_TX;
Lightvalve 11:82d8768d7351 113
Lightvalve 11:82d8768d7351 114 int num_err;
Lightvalve 11:82d8768d7351 115 int flag_err[8];
Lightvalve 11:82d8768d7351 116 int flag_err_old[8];
Lightvalve 11:82d8768d7351 117 int flag_err_rt;
Lightvalve 11:82d8768d7351 118
Lightvalve 11:82d8768d7351 119 int flag_ref_enable;
Lightvalve 11:82d8768d7351 120
Lightvalve 11:82d8768d7351 121 int flag_data_request[5];
Lightvalve 11:82d8768d7351 122
Lightvalve 11:82d8768d7351 123 ////////////////////////////////////////////////////////////////////////////////
Lightvalve 11:82d8768d7351 124 ////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
Lightvalve 11:82d8768d7351 125 ////////////////////////////////////////////////////////////////////////////////
Lightvalve 11:82d8768d7351 126
Lightvalve 11:82d8768d7351 127 double CUR_PRES_A_BAR;
Lightvalve 11:82d8768d7351 128 double CUR_PRES_B_BAR;
Lightvalve 11:82d8768d7351 129 double CUR_TORQUE_NM;
Lightvalve 11:82d8768d7351 130 double CUR_TORQUE_NM_PRESS;
Lightvalve 11:82d8768d7351 131
Lightvalve 11:82d8768d7351 132 double PRES_A_VREF;
Lightvalve 11:82d8768d7351 133 double PRES_B_VREF;
Lightvalve 11:82d8768d7351 134 double TORQUE_VREF;
Lightvalve 11:82d8768d7351 135
Lightvalve 11:82d8768d7351 136 double VALVE_PWM_RAW;
Lightvalve 11:82d8768d7351 137 double VALVE_PWM_RAW_FB;
Lightvalve 11:82d8768d7351 138 double VALVE_PWM_RAW_FF;
Lightvalve 11:82d8768d7351 139 int VALVE_PWM_VALVE_DZ;
Lightvalve 11:82d8768d7351 140 int VALVE_INPUT_PWM;
Lightvalve 11:82d8768d7351 141
Lightvalve 11:82d8768d7351 142 double VALVE_GAIN_LPM_PER_V[10];
Lightvalve 11:82d8768d7351 143 double VALVE_POS_VS_PWM[18];
Lightvalve 11:82d8768d7351 144 long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 145
Lightvalve 11:82d8768d7351 146 int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 147 int VALVE_MIN_POS;
Lightvalve 11:82d8768d7351 148 int DDV_CENTER;
Lightvalve 11:82d8768d7351 149 int VALVE_POS_NUM;
Lightvalve 11:82d8768d7351 150
Lightvalve 14:8e7590227d22 151 int TMR3_COUNT_FINDHOME;
Lightvalve 14:8e7590227d22 152 int TMR3_COUNT_FLOWRATE;
Lightvalve 14:8e7590227d22 153 int TMR3_COUNT_DEADZONE;
Lightvalve 14:8e7590227d22 154 int TMR3_COUNT_PRES_NULL;
Lightvalve 14:8e7590227d22 155 int TMR3_COUNT_TORQUE_NULL;
Lightvalve 14:8e7590227d22 156 int TMR3_COUNT_PRES_CALIB;
Lightvalve 14:8e7590227d22 157 int TMR3_COUNT_REFERENCE;
Lightvalve 14:8e7590227d22 158 int TMR3_COUNT_JOINT;
Lightvalve 14:8e7590227d22 159 int TMR3_COUNT_ROTARY_FRIC_TUNE;
Lightvalve 11:82d8768d7351 160
Lightvalve 11:82d8768d7351 161 bool FLAG_REFERENCE_VALVE_PWM;
Lightvalve 11:82d8768d7351 162 bool FLAG_REFERENCE_VALVE_POSITION;
Lightvalve 11:82d8768d7351 163 bool FLAG_REFERENCE_JOINT_POSITION;
Lightvalve 11:82d8768d7351 164 bool FLAG_REFERENCE_JOINT_TORQUE;
Lightvalve 11:82d8768d7351 165 bool FLAG_REFERENCE_PRES_DIFF;
Lightvalve 14:8e7590227d22 166 bool FLAG_REFERENCE_CURRENT;
Lightvalve 11:82d8768d7351 167
Lightvalve 11:82d8768d7351 168 double TUNING_TIME;
Lightvalve 11:82d8768d7351 169
Lightvalve 11:82d8768d7351 170 double REFERENCE_FREQ;
Lightvalve 11:82d8768d7351 171 double REFERENCE_MAG;
Lightvalve 11:82d8768d7351 172
Lightvalve 11:82d8768d7351 173 bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 174
Lightvalve 11:82d8768d7351 175 int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 176 enum _JUMP_STATUS {
Lightvalve 11:82d8768d7351 177 JUMP_NO_ACT = 0, //0
Lightvalve 11:82d8768d7351 178 JUMP_START, //1
Lightvalve 11:82d8768d7351 179 JUMP_TAKEOFF, //2
Lightvalve 11:82d8768d7351 180 JUMP_FLYING, //3
Lightvalve 11:82d8768d7351 181 JUMP_LANDING, //4
Lightvalve 11:82d8768d7351 182 };
Lightvalve 11:82d8768d7351 183
Lightvalve 13:747daba9cf59 184 double CUR_PRES_DIFF_BAR = 0.0;
Lightvalve 13:747daba9cf59 185 double CUR_PRES_A_sum = 0.0;
Lightvalve 13:747daba9cf59 186 double CUR_PRES_B_sum = 0.0;
Lightvalve 13:747daba9cf59 187 double CUR_PRES_A_mean = 0.0;
Lightvalve 13:747daba9cf59 188 double CUR_PRES_B_mean = 0.0;
Lightvalve 13:747daba9cf59 189 double CUR_TORQUE_sum = 0.0;
Lightvalve 13:747daba9cf59 190 double CUR_TORQUE_mean = 0.0;
Lightvalve 13:747daba9cf59 191 double PRES_A_NULL = 200.0;
Lightvalve 13:747daba9cf59 192 double PRES_B_NULL = 200.0;
Lightvalve 13:747daba9cf59 193 double TORQUE_NULL = 3900;
Lightvalve 11:82d8768d7351 194
Lightvalve 13:747daba9cf59 195 double Ref_Valve_Pos_Old = 0.0;
Lightvalve 11:82d8768d7351 196
Lightvalve 13:747daba9cf59 197 int VALVE_ID_timer = 0;
Lightvalve 13:747daba9cf59 198 int VALVE_DZ_timer = 0;
Lightvalve 13:747daba9cf59 199 int VALVE_FR_timer = 0;
Lightvalve 13:747daba9cf59 200 int VALVE_POS_TMP = 0;
Lightvalve 13:747daba9cf59 201 int JOINT_VEL_TMP = 0;
Lightvalve 13:747daba9cf59 202 int DDV_POS_AVG = 0;
Lightvalve 13:747daba9cf59 203 int VALVE_POS_AVG[100] = {0};
Lightvalve 13:747daba9cf59 204 int VALVE_POS_AVG_OLD = 0;
Lightvalve 13:747daba9cf59 205 int data_num = 0;
Lightvalve 13:747daba9cf59 206 int ID_index = 0;
Lightvalve 13:747daba9cf59 207 int DZ_index = 1;
Lightvalve 13:747daba9cf59 208 int ID_index_array[100] = {0};
Lightvalve 13:747daba9cf59 209 int FR_index_array[100] = {0};
Lightvalve 13:747daba9cf59 210 int first_check = 0;
Lightvalve 13:747daba9cf59 211 double init_time = 0.0;
Lightvalve 13:747daba9cf59 212 int VEL_POINT = 0;
Lightvalve 13:747daba9cf59 213 int DZ_case = 0;
Lightvalve 13:747daba9cf59 214 int START_POS = 0;
Lightvalve 13:747daba9cf59 215 int FINAL_POS = 0;
Lightvalve 13:747daba9cf59 216 int DZ_DIRECTION = 0;
Lightvalve 13:747daba9cf59 217 int FIRST_DZ = 0;
Lightvalve 13:747daba9cf59 218 int SECOND_DZ = 0;
Lightvalve 13:747daba9cf59 219 int DZ_NUM = 0;
Lightvalve 13:747daba9cf59 220 int one_period_end = 0;
Lightvalve 13:747daba9cf59 221 double Ref_Vel_Test = 0.0;
Lightvalve 13:747daba9cf59 222 long TMR2_FOR_SLOW_LOGGING = 0;
Lightvalve 13:747daba9cf59 223 int velcount = 0;
Lightvalve 13:747daba9cf59 224 char max_check = 0;
Lightvalve 13:747daba9cf59 225 char min_check = 0;
Lightvalve 13:747daba9cf59 226
Lightvalve 13:747daba9cf59 227 double valve_pos_err = 0.0, valve_pos_err_old = 0.0, valve_pos_err_diff = 0.0, valve_pos_err_sum = 0.0;
Lightvalve 14:8e7590227d22 228 double joint_pos_err = 0.0, joint_pos_err_old = 0.0, joint_pos_err_diff = 0.0, joint_pos_err_diff_fil = 0.0, joint_pos_err_sum = 0.0;
Lightvalve 13:747daba9cf59 229 double joint_torq_err = 0.0, joint_torq_err_old = 0.0, joint_torq_err_diff = 0.0, joint_torq_err_sum = 0.0;
Lightvalve 13:747daba9cf59 230 double VALVE_PWM_RAW_POS = 0.0, VALVE_PWM_RAW_TORQ = 0.0;
Lightvalve 13:747daba9cf59 231
Lightvalve 13:747daba9cf59 232 double CUR_FLOWRATE = 0.0;
Lightvalve 13:747daba9cf59 233 double VALVE_FF_VOLTAGE = 0.0;
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 13:747daba9cf59 242 double CUR_VELOCITY_sum = 0.0;
Lightvalve 13:747daba9cf59 243 double temp_vel_sum = 0.0;
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
Lightvalve 13:747daba9cf59 254 // find home
Lightvalve 13:747daba9cf59 255 int CUR_VELOCITY_OLD = 0;
Lightvalve 13:747daba9cf59 256 int cnt_findhome = 0;
Lightvalve 13:747daba9cf59 257 int cnt_vel_findhome = 0;
Lightvalve 13:747daba9cf59 258 int FINDHOME_VELOCITY = 0;
Lightvalve 13:747daba9cf59 259 int FINDHOME_VELOCITY_OLD = 0;
Lightvalve 13:747daba9cf59 260 int FINDHOME_POSITION = 0;
Lightvalve 13:747daba9cf59 261 int FINDHOME_POSITION_OLD = 0;
Lightvalve 13:747daba9cf59 262
Lightvalve 13:747daba9cf59 263 // valve gain
Lightvalve 13:747daba9cf59 264 int check_vel_pos_init = 0;
Lightvalve 13:747daba9cf59 265 int check_vel_pos_fin = 0;
Lightvalve 13:747daba9cf59 266 int check_vel_pos_interv = 0;
Lightvalve 13:747daba9cf59 267 int valve_gain_repeat_cnt = 0;
Lightvalve 13:747daba9cf59 268 double VALVE_VOLTAGE = 0.0;
Lightvalve 13:747daba9cf59 269
Lightvalve 13:747daba9cf59 270 double freq_fric_tune = 1.0;
Lightvalve 13:747daba9cf59 271
Lightvalve 13:747daba9cf59 272 bool FLAG_VALVE_OUTPUT_CALIB = false;
Lightvalve 11:82d8768d7351 273
Lightvalve 14:8e7590227d22 274 uint32_t TMR3_COUNT_CAN_TX = 0;
Lightvalve 14:8e7590227d22 275
Lightvalve 14:8e7590227d22 276 double I_REF = 0.0;
Lightvalve 14:8e7590227d22 277 int TMR3_COUNT_IREF = 0;
Lightvalve 14:8e7590227d22 278 double CUR_CURRENT = 0.0;
Lightvalve 14:8e7590227d22 279 double u_CUR[3] = {0.0,0.0,0.0};
Lightvalve 14:8e7590227d22 280
Lightvalve 11:82d8768d7351 281 /*******************************************************************************
Lightvalve 11:82d8768d7351 282 * General math functions
Lightvalve 11:82d8768d7351 283 ******************************************************************************/
Lightvalve 11:82d8768d7351 284
Lightvalve 11:82d8768d7351 285
Lightvalve 11:82d8768d7351 286 double dabs(double tx) {
Lightvalve 11:82d8768d7351 287 if (tx >= 0.0)
Lightvalve 11:82d8768d7351 288 return tx;
Lightvalve 11:82d8768d7351 289 else
Lightvalve 11:82d8768d7351 290 return -tx;
Lightvalve 11:82d8768d7351 291 }
Lightvalve 11:82d8768d7351 292
Lightvalve 11:82d8768d7351 293 double change_int_to_efloat(int input) {
Lightvalve 11:82d8768d7351 294 int i = 0;
Lightvalve 11:82d8768d7351 295
Lightvalve 11:82d8768d7351 296 double output = 0;
Lightvalve 11:82d8768d7351 297 int vn = (int) ((double) input / 10.);
Lightvalve 11:82d8768d7351 298 int en = input % 10;
Lightvalve 11:82d8768d7351 299
Lightvalve 11:82d8768d7351 300 double temp = 1.;
Lightvalve 11:82d8768d7351 301 for (i = 0; i < en; i++)
Lightvalve 11:82d8768d7351 302 temp *= 0.1;
Lightvalve 11:82d8768d7351 303
Lightvalve 11:82d8768d7351 304 output = (double) vn*temp;
Lightvalve 11:82d8768d7351 305 return output;
Lightvalve 11:82d8768d7351 306 }
Lightvalve 11:82d8768d7351 307
Lightvalve 11:82d8768d7351 308 void make_delay(void) {
Lightvalve 11:82d8768d7351 309 int i = 0;
Lightvalve 11:82d8768d7351 310
Lightvalve 11:82d8768d7351 311 for (i = 0; i < 100000; i++) {
Lightvalve 11:82d8768d7351 312 ;
Lightvalve 11:82d8768d7351 313 }
Lightvalve 11:82d8768d7351 314
Lightvalve 11:82d8768d7351 315 }
Lightvalve 11:82d8768d7351 316
Lightvalve 11:82d8768d7351 317
Lightvalve 11:82d8768d7351 318 /*******************************************************************************
Lightvalve 11:82d8768d7351 319 * ROM functions
Lightvalve 11:82d8768d7351 320 ******************************************************************************/
Lightvalve 14:8e7590227d22 321 void ROM_INIT_DATA(void)
Lightvalve 14:8e7590227d22 322 {
Lightvalve 15:bd0d12728506 323 // static int16_t temp_check_num = LATEST_VERSION;
Lightvalve 15:bd0d12728506 324 // spi_eeprom_ready();
Lightvalve 15:bd0d12728506 325 // if (spi_eeprom_read(RID_IS_FIRST) != temp_check_num) {
Lightvalve 15:bd0d12728506 326 // spi_eeprom_ready();
Lightvalve 15:bd0d12728506 327 // spi_eeprom_write(RID_IS_FIRST, temp_check_num);
Lightvalve 15:bd0d12728506 328 // ROM_RESET_DATA();
Lightvalve 15:bd0d12728506 329 // }
Lightvalve 11:82d8768d7351 330 ROM_CALL_DATA();
Lightvalve 15:bd0d12728506 331 ROM_RESET_DATA();
Lightvalve 11:82d8768d7351 332 }
Lightvalve 11:82d8768d7351 333
Lightvalve 11:82d8768d7351 334 void ROM_RESET_DATA(void) {
Lightvalve 15:bd0d12728506 335 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 336 spi_eeprom_write(RID_BNO, (int) BNO);
Lightvalve 15:bd0d12728506 337 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 338 BNO = spi_eeprom_read(RID_BNO);
Lightvalve 15:bd0d12728506 339 BNO = 11;
Lightvalve 15:bd0d12728506 340
Lightvalve 15:bd0d12728506 341 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 342 spi_eeprom_write(RID_OPERATING_MODE, (int) OPERATING_MODE);
Lightvalve 15:bd0d12728506 343 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 344 OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
Lightvalve 15:bd0d12728506 345
Lightvalve 15:bd0d12728506 346 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 347 spi_eeprom_write(RID_CAN_FREQ, (int) CAN_FREQ);
Lightvalve 15:bd0d12728506 348 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 349 CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
Lightvalve 15:bd0d12728506 350
Lightvalve 15:bd0d12728506 351 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 352 spi_eeprom_write(RID_JOINT_ENC_DIR, (int) DIR_JOINT_ENC);
Lightvalve 15:bd0d12728506 353 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 354 DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
Lightvalve 15:bd0d12728506 355
Lightvalve 15:bd0d12728506 356 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 357 spi_eeprom_write(RID_VALVE_DIR, (int) DIR_VALVE);
Lightvalve 15:bd0d12728506 358 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 359 DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
Lightvalve 15:bd0d12728506 360
Lightvalve 15:bd0d12728506 361 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 362 spi_eeprom_write(RID_VALVE_ENC_DIR, (int) DIR_VALVE_ENC);
Lightvalve 15:bd0d12728506 363 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 364 DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
Lightvalve 15:bd0d12728506 365
Lightvalve 15:bd0d12728506 366 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 367 spi_eeprom_write(RID_VOLATGE_SUPPLY, (int) (SUPPLY_VOLTAGE * 10.0));
Lightvalve 15:bd0d12728506 368 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 369 SUPPLY_VOLTAGE = (double) spi_eeprom_read(RID_VOLATGE_SUPPLY) / 10.0;
Lightvalve 15:bd0d12728506 370
Lightvalve 15:bd0d12728506 371 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 372 spi_eeprom_write(RID_VOLTAGE_VALVE, (int) (VALVE_VOLTAGE_LIMIT * 10.0));
Lightvalve 15:bd0d12728506 373 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 374 VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.0;
Lightvalve 15:bd0d12728506 375
Lightvalve 15:bd0d12728506 376 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 377 spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int) P_GAIN_VALVE_POSITION);
Lightvalve 15:bd0d12728506 378 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 379 P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
Lightvalve 15:bd0d12728506 380
Lightvalve 15:bd0d12728506 381 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 382 spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int) I_GAIN_VALVE_POSITION);
Lightvalve 15:bd0d12728506 383 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 384 I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
Lightvalve 15:bd0d12728506 385
Lightvalve 15:bd0d12728506 386 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 387 spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int) D_GAIN_VALVE_POSITION);
Lightvalve 15:bd0d12728506 388 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 389 D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
Lightvalve 15:bd0d12728506 390
Lightvalve 15:bd0d12728506 391 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 392 spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int) P_GAIN_JOINT_POSITION);
Lightvalve 15:bd0d12728506 393 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 394 P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
Lightvalve 15:bd0d12728506 395
Lightvalve 15:bd0d12728506 396 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 397 spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int) I_GAIN_JOINT_POSITION);
Lightvalve 15:bd0d12728506 398 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 399 I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
Lightvalve 15:bd0d12728506 400
Lightvalve 15:bd0d12728506 401 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 402 spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int) D_GAIN_JOINT_POSITION);
Lightvalve 15:bd0d12728506 403 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 404 D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
Lightvalve 15:bd0d12728506 405
Lightvalve 15:bd0d12728506 406 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 407 spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int) P_GAIN_JOINT_TORQUE);
Lightvalve 15:bd0d12728506 408 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 409 P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
Lightvalve 15:bd0d12728506 410
Lightvalve 15:bd0d12728506 411 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 412 spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int) I_GAIN_JOINT_TORQUE);
Lightvalve 15:bd0d12728506 413 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 414 I_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_I_GAIN_JOINT_TORQUE);
Lightvalve 15:bd0d12728506 415
Lightvalve 15:bd0d12728506 416 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 417 spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int) D_GAIN_JOINT_TORQUE);
Lightvalve 15:bd0d12728506 418 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 419 D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
Lightvalve 15:bd0d12728506 420
Lightvalve 15:bd0d12728506 421 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 422 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int) (VALVE_DEADZONE_PLUS));
Lightvalve 15:bd0d12728506 423 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 424 VALVE_DEADZONE_PLUS = spi_eeprom_read(RID_VALVE_DEADZONE_PLUS);
Lightvalve 15:bd0d12728506 425
Lightvalve 15:bd0d12728506 426 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 427 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int) (VALVE_DEADZONE_MINUS));
Lightvalve 15:bd0d12728506 428 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 429 VALVE_DEADZONE_MINUS = spi_eeprom_read(RID_VALVE_DEADZONE_MINUS);
Lightvalve 15:bd0d12728506 430
Lightvalve 15:bd0d12728506 431 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 432 spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int) VELOCITY_COMP_GAIN);
Lightvalve 15:bd0d12728506 433 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 434 VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
Lightvalve 15:bd0d12728506 435
Lightvalve 15:bd0d12728506 436 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 437 spi_eeprom_write(RID_COMPLIANCE_GAIN, (int) COMPLIANCE_GAIN);
Lightvalve 15:bd0d12728506 438 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 439 COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
Lightvalve 15:bd0d12728506 440
Lightvalve 15:bd0d12728506 441 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 442 spi_eeprom_write(RID_VALVE_CNETER, (int) VALVE_CENTER);
Lightvalve 15:bd0d12728506 443 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 444 VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
Lightvalve 15:bd0d12728506 445
Lightvalve 15:bd0d12728506 446 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 447 spi_eeprom_write(RID_VALVE_FF, (int) VALVE_FF);
Lightvalve 15:bd0d12728506 448 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 449 VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
Lightvalve 15:bd0d12728506 450
Lightvalve 15:bd0d12728506 451 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 452 spi_eeprom_write(RID_BULK_MODULUS, (int) BULK_MODULUS);
Lightvalve 15:bd0d12728506 453 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 454 BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
Lightvalve 15:bd0d12728506 455
Lightvalve 15:bd0d12728506 456 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 457 spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int) CHAMBER_VOLUME_A);
Lightvalve 15:bd0d12728506 458 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 459 CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
Lightvalve 15:bd0d12728506 460
Lightvalve 15:bd0d12728506 461 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 462 spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int) CHAMBER_VOLUME_B);
Lightvalve 15:bd0d12728506 463 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 464 CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
Lightvalve 15:bd0d12728506 465
Lightvalve 15:bd0d12728506 466 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 467 spi_eeprom_write(RID_PISTON_AREA_A, (int) PISTON_AREA_A);
Lightvalve 15:bd0d12728506 468 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 469 PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
Lightvalve 15:bd0d12728506 470
Lightvalve 15:bd0d12728506 471 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 472 spi_eeprom_write(RID_PISTON_AREA_B, (int) PISTON_AREA_B);
Lightvalve 15:bd0d12728506 473 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 474 PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
Lightvalve 15:bd0d12728506 475
Lightvalve 15:bd0d12728506 476 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 477 spi_eeprom_write(RID_PRES_SUPPLY, (int) PRES_SUPPLY);
Lightvalve 15:bd0d12728506 478 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 479 PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
Lightvalve 15:bd0d12728506 480
Lightvalve 15:bd0d12728506 481 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 482 spi_eeprom_write(RID_PRES_RETURN, (int) PRES_RETURN);
Lightvalve 15:bd0d12728506 483 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 484 PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
Lightvalve 15:bd0d12728506 485
Lightvalve 15:bd0d12728506 486 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 487 spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int) ENC_LIMIT_MINUS);
Lightvalve 15:bd0d12728506 488 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 489 ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
Lightvalve 15:bd0d12728506 490
Lightvalve 15:bd0d12728506 491 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 492 spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int) ENC_LIMIT_PLUS);
Lightvalve 15:bd0d12728506 493 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 494 ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
Lightvalve 15:bd0d12728506 495
Lightvalve 15:bd0d12728506 496 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 497 spi_eeprom_write(RID_STROKE, (int) STROKE);
Lightvalve 15:bd0d12728506 498 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 499 STROKE = spi_eeprom_read(RID_STROKE);
Lightvalve 15:bd0d12728506 500
Lightvalve 15:bd0d12728506 501 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 502 spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int) VALVE_LIMIT_MINUS);
Lightvalve 15:bd0d12728506 503 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 504 VALVE_LIMIT_MINUS = spi_eeprom_read(RID_VALVE_LIMIT_MINUS);
Lightvalve 15:bd0d12728506 505
Lightvalve 15:bd0d12728506 506 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 507 spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int) VALVE_LIMIT_PLUS);
Lightvalve 15:bd0d12728506 508 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 509 VALVE_LIMIT_PLUS = spi_eeprom_read(RID_VALVE_LIMIT_PLUS);
Lightvalve 15:bd0d12728506 510
Lightvalve 15:bd0d12728506 511 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 512 spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int) ENC_PULSE_PER_POSITION);
Lightvalve 15:bd0d12728506 513 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 514 ENC_PULSE_PER_POSITION = spi_eeprom_read(RID_ENC_PULSE_PER_POSITION);
Lightvalve 15:bd0d12728506 515
Lightvalve 15:bd0d12728506 516 TORQUE_SENSOR_PULSE_PER_TORQUE = 4096.0 / 200.0;
Lightvalve 15:bd0d12728506 517 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 518 spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int) TORQUE_SENSOR_PULSE_PER_TORQUE);
Lightvalve 15:bd0d12728506 519 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 520 TORQUE_SENSOR_PULSE_PER_TORQUE = spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
Lightvalve 15:bd0d12728506 521
Lightvalve 15:bd0d12728506 522 PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
Lightvalve 15:bd0d12728506 523 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 524 spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
Lightvalve 15:bd0d12728506 525 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 526 PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
Lightvalve 11:82d8768d7351 527
Lightvalve 15:bd0d12728506 528 PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
Lightvalve 15:bd0d12728506 529 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 530 spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
Lightvalve 15:bd0d12728506 531 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 532 PRES_SENSOR_B_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR) * 0.01;
Lightvalve 15:bd0d12728506 533
Lightvalve 15:bd0d12728506 534 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 535 spi_eeprom_write(RID_FRICTION, (int) (FRICTION * 10.0));
Lightvalve 15:bd0d12728506 536 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 537 FRICTION = (double) spi_eeprom_read(RID_FRICTION) / 10.0;
Lightvalve 15:bd0d12728506 538
Lightvalve 15:bd0d12728506 539 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 540 spi_eeprom_write(RID_HOMEPOS_OFFSET, (int) HOMEPOS_OFFSET);
Lightvalve 15:bd0d12728506 541 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 542 HOMEPOS_OFFSET = (int) spi_eeprom_read(RID_HOMEPOS_OFFSET);
Lightvalve 15:bd0d12728506 543
Lightvalve 15:bd0d12728506 544 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 545 spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int) HOMEPOS_VALVE_OPENING);
Lightvalve 15:bd0d12728506 546 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 547 HOMEPOS_VALVE_OPENING = (int) spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
Lightvalve 15:bd0d12728506 548
Lightvalve 15:bd0d12728506 549 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 550 spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int) (TORQUE_VREF * 1000.0));
Lightvalve 15:bd0d12728506 551 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 552 TORQUE_VREF = (double) spi_eeprom_read(RID_TORQUE_SENSOR_VREF) / 1000.0;
Lightvalve 15:bd0d12728506 553
Lightvalve 15:bd0d12728506 554 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 555 spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0));
Lightvalve 15:bd0d12728506 556 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 557 PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.0;
Lightvalve 15:bd0d12728506 558
Lightvalve 15:bd0d12728506 559 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 560 spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0));
Lightvalve 15:bd0d12728506 561 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 562 PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.0;
Lightvalve 15:bd0d12728506 563
Lightvalve 15:bd0d12728506 564 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 565 spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int) (VALVE_GAIN_LPM_PER_V[0] * 100.0));
Lightvalve 15:bd0d12728506 566 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 567 VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.0;
Lightvalve 15:bd0d12728506 568
Lightvalve 14:8e7590227d22 569 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 570 spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int) (VALVE_GAIN_LPM_PER_V[2] * 100.0));
Lightvalve 15:bd0d12728506 571 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 572 VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.0;
Lightvalve 15:bd0d12728506 573
Lightvalve 15:bd0d12728506 574 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 575 spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int) (VALVE_GAIN_LPM_PER_V[4] * 100.0));
Lightvalve 15:bd0d12728506 576 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 577 VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.0;
Lightvalve 15:bd0d12728506 578
Lightvalve 15:bd0d12728506 579 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 580 spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int) (VALVE_GAIN_LPM_PER_V[6] * 100.0));
Lightvalve 15:bd0d12728506 581 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 582 VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.0;
Lightvalve 15:bd0d12728506 583
Lightvalve 15:bd0d12728506 584 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 585 spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int) (VALVE_GAIN_LPM_PER_V[8] * 100.0));
Lightvalve 15:bd0d12728506 586 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 587 VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.0;
Lightvalve 15:bd0d12728506 588
Lightvalve 15:bd0d12728506 589 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 590 spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int) (VALVE_GAIN_LPM_PER_V[1] * 100.0));
Lightvalve 14:8e7590227d22 591 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 592 VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.0;
Lightvalve 15:bd0d12728506 593
Lightvalve 15:bd0d12728506 594 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 595 spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int) (VALVE_GAIN_LPM_PER_V[3] * 100.0));
Lightvalve 15:bd0d12728506 596 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 597 VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.0;
Lightvalve 11:82d8768d7351 598
Lightvalve 15:bd0d12728506 599 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 600 spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int) (VALVE_GAIN_LPM_PER_V[5] * 100.0));
Lightvalve 15:bd0d12728506 601 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 602 VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.0;
Lightvalve 15:bd0d12728506 603
Lightvalve 15:bd0d12728506 604 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 605 spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int) (VALVE_GAIN_LPM_PER_V[7] * 100.0));
Lightvalve 15:bd0d12728506 606 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 607 VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.0;
Lightvalve 15:bd0d12728506 608
Lightvalve 15:bd0d12728506 609 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 610 spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int) (VALVE_GAIN_LPM_PER_V[9] * 100.0));
Lightvalve 15:bd0d12728506 611 spi_eeprom_ready();
Lightvalve 15:bd0d12728506 612 VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.0;
Lightvalve 11:82d8768d7351 613
Lightvalve 11:82d8768d7351 614 }
Lightvalve 11:82d8768d7351 615
Lightvalve 11:82d8768d7351 616 void ROM_CALL_DATA(void) {
Lightvalve 11:82d8768d7351 617 int i = 0;
Lightvalve 11:82d8768d7351 618 BNO = spi_eeprom_read(RID_BNO);
Lightvalve 14:8e7590227d22 619 BNO = 11;
Lightvalve 11:82d8768d7351 620 OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
Lightvalve 11:82d8768d7351 621 CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
Lightvalve 11:82d8768d7351 622
Lightvalve 11:82d8768d7351 623 DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
Lightvalve 11:82d8768d7351 624 DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
Lightvalve 11:82d8768d7351 625 DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
Lightvalve 11:82d8768d7351 626
Lightvalve 14:8e7590227d22 627 SUPPLY_VOLTAGE = (double) spi_eeprom_read(RID_VOLATGE_SUPPLY) / 10.0;
Lightvalve 14:8e7590227d22 628 VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.0;
Lightvalve 11:82d8768d7351 629
Lightvalve 11:82d8768d7351 630 P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 631 I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 632 D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 633
Lightvalve 11:82d8768d7351 634 P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 635 I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 636 D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 637
Lightvalve 11:82d8768d7351 638 P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 639 I_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_I_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 640 D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 641
Lightvalve 11:82d8768d7351 642 VALVE_DEADZONE_PLUS = spi_eeprom_read(RID_VALVE_DEADZONE_PLUS);
Lightvalve 11:82d8768d7351 643 VALVE_DEADZONE_MINUS = spi_eeprom_read(RID_VALVE_DEADZONE_MINUS);
Lightvalve 11:82d8768d7351 644
Lightvalve 11:82d8768d7351 645 VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
Lightvalve 11:82d8768d7351 646 COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
Lightvalve 11:82d8768d7351 647
Lightvalve 11:82d8768d7351 648 VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
Lightvalve 11:82d8768d7351 649 VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
Lightvalve 11:82d8768d7351 650 BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
Lightvalve 11:82d8768d7351 651
Lightvalve 11:82d8768d7351 652 CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
Lightvalve 11:82d8768d7351 653 CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
Lightvalve 11:82d8768d7351 654
Lightvalve 11:82d8768d7351 655 PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
Lightvalve 11:82d8768d7351 656 PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
Lightvalve 11:82d8768d7351 657 PISTON_AREA_alpha = (double)PISTON_AREA_B/(double)PISTON_AREA_A;
Lightvalve 11:82d8768d7351 658
Lightvalve 11:82d8768d7351 659 PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
Lightvalve 11:82d8768d7351 660 PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
Lightvalve 11:82d8768d7351 661
Lightvalve 11:82d8768d7351 662 ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
Lightvalve 11:82d8768d7351 663 ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 664
Lightvalve 11:82d8768d7351 665 STROKE = spi_eeprom_read(RID_STROKE);
Lightvalve 11:82d8768d7351 666
Lightvalve 11:82d8768d7351 667 VALVE_LIMIT_MINUS = spi_eeprom_read(RID_VALVE_LIMIT_MINUS);
Lightvalve 11:82d8768d7351 668 VALVE_LIMIT_PLUS = spi_eeprom_read(RID_VALVE_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 669
Lightvalve 11:82d8768d7351 670 ENC_PULSE_PER_POSITION = spi_eeprom_read(RID_ENC_PULSE_PER_POSITION);
Lightvalve 11:82d8768d7351 671 TORQUE_SENSOR_PULSE_PER_TORQUE = spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
Lightvalve 15:bd0d12728506 672 spi_eeprom_ready();
Lightvalve 11:82d8768d7351 673 PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
Lightvalve 15:bd0d12728506 674 spi_eeprom_ready();
Lightvalve 11:82d8768d7351 675 PRES_SENSOR_B_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR) * 0.01;
Lightvalve 11:82d8768d7351 676
Lightvalve 14:8e7590227d22 677 FRICTION = (double) spi_eeprom_read(RID_FRICTION) / 10.0;
Lightvalve 11:82d8768d7351 678 HOMEPOS_OFFSET = (int) spi_eeprom_read(RID_HOMEPOS_OFFSET);
Lightvalve 11:82d8768d7351 679 HOMEPOS_VALVE_OPENING = (int) spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
Lightvalve 11:82d8768d7351 680
Lightvalve 14:8e7590227d22 681 TORQUE_VREF = (double) spi_eeprom_read(RID_TORQUE_SENSOR_VREF) / 1000.0;
Lightvalve 11:82d8768d7351 682
Lightvalve 14:8e7590227d22 683 PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.0;
Lightvalve 14:8e7590227d22 684 PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.0;
Lightvalve 11:82d8768d7351 685
Lightvalve 14:8e7590227d22 686 VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.0;
Lightvalve 14:8e7590227d22 687 VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.0;
Lightvalve 14:8e7590227d22 688 VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.0;
Lightvalve 14:8e7590227d22 689 VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.0;
Lightvalve 14:8e7590227d22 690 VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.0;
Lightvalve 14:8e7590227d22 691 VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.0;
Lightvalve 14:8e7590227d22 692 VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.0;
Lightvalve 14:8e7590227d22 693 VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.0;
Lightvalve 14:8e7590227d22 694 VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.0;
Lightvalve 14:8e7590227d22 695 VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.0;
Lightvalve 11:82d8768d7351 696
Lightvalve 11:82d8768d7351 697 for(i=0; i<18; i++)
Lightvalve 11:82d8768d7351 698 {
Lightvalve 11:82d8768d7351 699 VALVE_POS_VS_PWM[i] = (double) spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i);
Lightvalve 11:82d8768d7351 700 }
Lightvalve 11:82d8768d7351 701 for(i=0; i<100; i++)
Lightvalve 11:82d8768d7351 702 {
Lightvalve 12:6f2531038ea4 703 JOINT_VEL[i] = (int32_t) ( (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 11:82d8768d7351 704 }
Lightvalve 11:82d8768d7351 705
Lightvalve 11:82d8768d7351 706 VALVE_MAX_POS = (int) spi_eeprom_read(RID_VALVE_MAX_POS);
Lightvalve 11:82d8768d7351 707 VALVE_MIN_POS = (int) spi_eeprom_read(RID_VALVE_MIN_POS);
Lightvalve 11:82d8768d7351 708 DDV_CENTER = (int) spi_eeprom_read(RID_DDV_CNETER);
Lightvalve 11:82d8768d7351 709
Lightvalve 11:82d8768d7351 710 VALVE_POS_NUM = (int) spi_eeprom_read(RID_VALVE_POS_NUM);
Lightvalve 11:82d8768d7351 711
Lightvalve 11:82d8768d7351 712 }
Lightvalve 11:82d8768d7351 713
Lightvalve 11:82d8768d7351 714 /*******************************************************************************
Lightvalve 11:82d8768d7351 715 * ENCODER functions
Lightvalve 11:82d8768d7351 716 ******************************************************************************/
Lightvalve 11:82d8768d7351 717 // A-KHA
Lightvalve 11:82d8768d7351 718 #define KF_G1_11 0.083920206005350
Lightvalve 11:82d8768d7351 719 #define KF_G1_12 0.000013905329560
Lightvalve 11:82d8768d7351 720 #define KF_G1_21 -0.000575742328210
Lightvalve 11:82d8768d7351 721 #define KF_G1_22 0.799999939711725
Lightvalve 11:82d8768d7351 722 // K
Lightvalve 11:82d8768d7351 723 #define KF_G2_11 0.916079793994650
Lightvalve 11:82d8768d7351 724 #define KF_G2_12 0.000002878711641
Lightvalve 11:82d8768d7351 725 #define KF_G2_21 0.000575742328210
Lightvalve 11:82d8768d7351 726 #define KF_G2_22 0.199999945139809
Lightvalve 11:82d8768d7351 727
Lightvalve 11:82d8768d7351 728 double KF_Y_11 = 0.;
Lightvalve 11:82d8768d7351 729 double KF_Y_21 = 0.;
Lightvalve 11:82d8768d7351 730 double KF_X_11 = 0.;
Lightvalve 11:82d8768d7351 731 double KF_X_21 = 0.;
Lightvalve 11:82d8768d7351 732
Lightvalve 11:82d8768d7351 733 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
Lightvalve 11:82d8768d7351 734 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
Lightvalve 11:82d8768d7351 735 long enc_offset = 0;
Lightvalve 11:82d8768d7351 736
Lightvalve 11:82d8768d7351 737 void ENC_UPDATE(void) {
Lightvalve 11:82d8768d7351 738
Lightvalve 11:82d8768d7351 739 ENC_pos_cur = spi_enc_read();
Lightvalve 11:82d8768d7351 740 ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
Lightvalve 11:82d8768d7351 741
Lightvalve 11:82d8768d7351 742 if (ENC_pos_diff > 1300 || ENC_pos_diff<-1300) {
Lightvalve 11:82d8768d7351 743 //MOT_E_STOP(0);
Lightvalve 11:82d8768d7351 744 }
Lightvalve 11:82d8768d7351 745
Lightvalve 12:6f2531038ea4 746 ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
Lightvalve 11:82d8768d7351 747
Lightvalve 11:82d8768d7351 748 KF_Y_11 = ENC_pos_cur;
Lightvalve 11:82d8768d7351 749 KF_Y_21 = ENC_VEL_RAW;
Lightvalve 11:82d8768d7351 750 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 11:82d8768d7351 751 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 12:6f2531038ea4 752 ENC_VEL_KF = (int32_t) KF_X_21;
Lightvalve 11:82d8768d7351 753
Lightvalve 14:8e7590227d22 754 pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
Lightvalve 11:82d8768d7351 755 // CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
Lightvalve 14:8e7590227d22 756 vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
Lightvalve 11:82d8768d7351 757
Lightvalve 11:82d8768d7351 758 // CUR_POSITION = ENC_pos_cur;
Lightvalve 11:82d8768d7351 759 // CUR_VELOCITY = ENC_VEL_KF;
Lightvalve 11:82d8768d7351 760
Lightvalve 11:82d8768d7351 761 ENC_pos_old = ENC_pos_cur;
Lightvalve 11:82d8768d7351 762 }
Lightvalve 11:82d8768d7351 763
Lightvalve 11:82d8768d7351 764 void ENC_SET_ZERO(void) {
Lightvalve 11:82d8768d7351 765
Lightvalve 11:82d8768d7351 766 spi_enc_set_clear();
Lightvalve 11:82d8768d7351 767 CUR_POSITION = 0;
Lightvalve 11:82d8768d7351 768 ENC_pos_old = ENC_pos_cur = 0;
Lightvalve 11:82d8768d7351 769
Lightvalve 11:82d8768d7351 770 }
Lightvalve 11:82d8768d7351 771
Lightvalve 13:747daba9cf59 772 void ENC_SET(int32_t value_e) {
Lightvalve 11:82d8768d7351 773 spi_enc_set_clear();
Lightvalve 13:747daba9cf59 774 enc_offset = value_e;
Lightvalve 13:747daba9cf59 775 CUR_POSITION = value_e;
Lightvalve 13:747daba9cf59 776 ENC_pos_old = ENC_pos_cur = value_e;
Lightvalve 11:82d8768d7351 777
Lightvalve 11:82d8768d7351 778 }