Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard

asdgas

Dependencies:   mbed Eigen FastPWM

function_utilities/function_utilities.cpp@37:60a116154383, 2019-11-25 (annotated)

Committer:
Lightvalve
Date:
Mon Nov 25 05:04:12 2019 +0000
Revision:
37:60a116154383
Parent:
36:a46e63505ed8
Child:
38:118df027d851
191125

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