Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

Dependencies:   mbed FastPWM

function_utilities/function_utilities.cpp@16:903b5a4433b4, 2019-09-02 (annotated)

Committer:
Lightvalve
Date:
Mon Sep 02 13:32:33 2019 +0000
Revision:
16:903b5a4433b4
Parent:
15:bd0d12728506
Child:
17:1865016ca2e7
190902

Who changed what in which revision?

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