Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_PostLIGHT_210420

LIGHT2

Dependencies:   FastPWM mbed

function_utilities/function_utilities.cpp@227:699c3e572283, 2021-03-11 (annotated)

Committer:
hyhoon
Date:
Thu Mar 11 07:27:23 2021 +0000
Revision:
227:699c3e572283
Parent:
226:82a3ca333004
Child:
230:2c3e5ecbe7e1
210311

Who changed what in which revision?

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