Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_LIGHT

For LIGHT

Dependencies:   mbed FastPWM

function_utilities/function_utilities.cpp@12:6f2531038ea4, 2019-08-27 (annotated)

Committer:
Lightvalve
Date:
Tue Aug 27 05:32:44 2019 +0000
Revision:
12:6f2531038ea4
Parent:
11:82d8768d7351
Child:
13:747daba9cf59
190827

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 11:82d8768d7351 4
Lightvalve 11:82d8768d7351 5 /*******************************************************************************
Lightvalve 11:82d8768d7351 6 * VARIABLE
Lightvalve 11:82d8768d7351 7 ******************************************************************************/
Lightvalve 11:82d8768d7351 8
Lightvalve 11:82d8768d7351 9 // Board Information
Lightvalve 11:82d8768d7351 10 uint8_t BNO = 0;
Lightvalve 11:82d8768d7351 11 uint8_t CONTROL_MODE = 0;
Lightvalve 11:82d8768d7351 12 uint8_t OPERATING_MODE = 0;
Lightvalve 11:82d8768d7351 13 uint8_t SETTING_SWITCH = 0;
Lightvalve 11:82d8768d7351 14 uint8_t SETTING_SWITCH_OLD = 0;
Lightvalve 11:82d8768d7351 15 uint8_t REFERENCE_MODE = 0;
Lightvalve 11:82d8768d7351 16 uint16_t CAN_FREQ = 5000;
Lightvalve 11:82d8768d7351 17 uint8_t DIR_JOINT_ENC = 0;
Lightvalve 11:82d8768d7351 18 uint8_t DIR_VALVE = 0;
Lightvalve 11:82d8768d7351 19 uint8_t DIR_VALVE_ENC = 0;
Lightvalve 11:82d8768d7351 20
Lightvalve 11:82d8768d7351 21 double SUPPLY_VOLTAGE = 12.0;
Lightvalve 11:82d8768d7351 22 double VALVE_VOLTAGE_LIMIT = 5.0;
Lightvalve 11:82d8768d7351 23
Lightvalve 11:82d8768d7351 24 double P_GAIN_VALVE_POSITION = 0.0;
Lightvalve 11:82d8768d7351 25 double I_GAIN_VALVE_POSITION= 0.0;
Lightvalve 11:82d8768d7351 26 double D_GAIN_VALVE_POSITION= 0.0;
Lightvalve 11:82d8768d7351 27 double P_GAIN_JOINT_POSITION = 0.0;
Lightvalve 11:82d8768d7351 28 double I_GAIN_JOINT_POSITION = 0.0;
Lightvalve 11:82d8768d7351 29 double D_GAIN_JOINT_POSITION = 0.0;
Lightvalve 11:82d8768d7351 30 double P_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 31 double I_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 32 double D_GAIN_JOINT_TORQUE = 0.0;
Lightvalve 11:82d8768d7351 33
Lightvalve 11:82d8768d7351 34 int16_t VALVE_DEADZONE_PLUS;
Lightvalve 11:82d8768d7351 35 int16_t VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 36
Lightvalve 11:82d8768d7351 37 int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 38 int16_t COMPLIANCE_GAIN;
Lightvalve 11:82d8768d7351 39
Lightvalve 11:82d8768d7351 40 int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 41
Lightvalve 11:82d8768d7351 42 int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 43
Lightvalve 11:82d8768d7351 44 int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 45
Lightvalve 11:82d8768d7351 46 int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 47 int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 48
Lightvalve 11:82d8768d7351 49 int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 50 int16_t PISTON_AREA_B;
Lightvalve 11:82d8768d7351 51 double PISTON_AREA_alpha;
Lightvalve 11:82d8768d7351 52
Lightvalve 11:82d8768d7351 53
Lightvalve 11:82d8768d7351 54 int16_t PRES_SUPPLY;
Lightvalve 11:82d8768d7351 55 int16_t PRES_RETURN;
Lightvalve 11:82d8768d7351 56
Lightvalve 11:82d8768d7351 57 int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 58 int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 59
Lightvalve 11:82d8768d7351 60 int16_t STROKE;
Lightvalve 11:82d8768d7351 61
Lightvalve 11:82d8768d7351 62
Lightvalve 11:82d8768d7351 63 int16_t VALVE_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 64 int16_t VALVE_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 65
Lightvalve 11:82d8768d7351 66 int16_t ENC_PULSE_PER_POSITION;
Lightvalve 11:82d8768d7351 67 int16_t TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 11:82d8768d7351 68 double PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve 11:82d8768d7351 69 double PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve 11:82d8768d7351 70
Lightvalve 11:82d8768d7351 71 int HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 72 int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 73
Lightvalve 11:82d8768d7351 74 double FRICTION;
Lightvalve 11:82d8768d7351 75 double REF_PERIOD;
Lightvalve 11:82d8768d7351 76 double REF_MAG;
Lightvalve 11:82d8768d7351 77 int REF_NUM;
Lightvalve 11:82d8768d7351 78
Lightvalve 11:82d8768d7351 79
Lightvalve 11:82d8768d7351 80 double DAC_REF;
Lightvalve 11:82d8768d7351 81 double DAC_RESOL;
Lightvalve 11:82d8768d7351 82 double PWM_RESOL;
Lightvalve 11:82d8768d7351 83
Lightvalve 11:82d8768d7351 84 int REF_POSITION;
Lightvalve 11:82d8768d7351 85 int REF_VELOCITY;
Lightvalve 11:82d8768d7351 86 int16_t REF_TORQUE;
Lightvalve 11:82d8768d7351 87 int16_t REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 88 int16_t REF_PWM;
Lightvalve 11:82d8768d7351 89 int16_t REF_VALVE_POSITION;
Lightvalve 11:82d8768d7351 90
Lightvalve 11:82d8768d7351 91 int REF_MOVE_TIME_5k;
Lightvalve 11:82d8768d7351 92 int INIT_REF_PWM;
Lightvalve 11:82d8768d7351 93 int INIT_REF_VALVE_POS;
Lightvalve 11:82d8768d7351 94 int INIT_REF_POS;
Lightvalve 11:82d8768d7351 95 int INIT_REF_VEL;
Lightvalve 11:82d8768d7351 96 int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 97 int INIT_REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 98
Lightvalve 11:82d8768d7351 99 int CUR_POSITION;
Lightvalve 11:82d8768d7351 100 int CUR_VELOCITY;
Lightvalve 11:82d8768d7351 101 double CUR_TORQUE;
Lightvalve 11:82d8768d7351 102 double CUR_PRES_A;
Lightvalve 11:82d8768d7351 103 double CUR_PRES_B;
Lightvalve 11:82d8768d7351 104 int CUR_PWM;
Lightvalve 11:82d8768d7351 105 int CUR_VALVE_POSITION;
Lightvalve 11:82d8768d7351 106
Lightvalve 11:82d8768d7351 107 unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 108 unsigned int TMR2_COUNT_LED2;
Lightvalve 11:82d8768d7351 109 unsigned int TMR2_COUNT_CAN_TX;
Lightvalve 11:82d8768d7351 110
Lightvalve 11:82d8768d7351 111 int num_err;
Lightvalve 11:82d8768d7351 112 int flag_err[8];
Lightvalve 11:82d8768d7351 113 int flag_err_old[8];
Lightvalve 11:82d8768d7351 114 int flag_err_rt;
Lightvalve 11:82d8768d7351 115
Lightvalve 11:82d8768d7351 116 int flag_ref_enable;
Lightvalve 11:82d8768d7351 117
Lightvalve 11:82d8768d7351 118 int flag_data_request[5];
Lightvalve 11:82d8768d7351 119
Lightvalve 11:82d8768d7351 120 ////////////////////////////////////////////////////////////////////////////////
Lightvalve 11:82d8768d7351 121 ////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
Lightvalve 11:82d8768d7351 122 ////////////////////////////////////////////////////////////////////////////////
Lightvalve 11:82d8768d7351 123
Lightvalve 11:82d8768d7351 124 double CUR_PRES_A_BAR;
Lightvalve 11:82d8768d7351 125 double CUR_PRES_B_BAR;
Lightvalve 11:82d8768d7351 126 double CUR_TORQUE_NM;
Lightvalve 11:82d8768d7351 127 double CUR_TORQUE_NM_PRESS;
Lightvalve 11:82d8768d7351 128
Lightvalve 11:82d8768d7351 129 double PRES_A_VREF;
Lightvalve 11:82d8768d7351 130 double PRES_B_VREF;
Lightvalve 11:82d8768d7351 131 double TORQUE_VREF;
Lightvalve 11:82d8768d7351 132
Lightvalve 11:82d8768d7351 133 double VALVE_PWM_RAW;
Lightvalve 11:82d8768d7351 134 double VALVE_PWM_RAW_FB;
Lightvalve 11:82d8768d7351 135 double VALVE_PWM_RAW_FF;
Lightvalve 11:82d8768d7351 136 int VALVE_PWM_VALVE_DZ;
Lightvalve 11:82d8768d7351 137 int VALVE_INPUT_PWM;
Lightvalve 11:82d8768d7351 138
Lightvalve 11:82d8768d7351 139 double VALVE_GAIN_LPM_PER_V[10];
Lightvalve 11:82d8768d7351 140 double VALVE_POS_VS_PWM[18];
Lightvalve 11:82d8768d7351 141 long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 142
Lightvalve 11:82d8768d7351 143 int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 144 int VALVE_MIN_POS;
Lightvalve 11:82d8768d7351 145 int DDV_CENTER;
Lightvalve 11:82d8768d7351 146 int VALVE_POS_NUM;
Lightvalve 11:82d8768d7351 147
Lightvalve 11:82d8768d7351 148 int TMR2_COUNT_FINDHOME;
Lightvalve 11:82d8768d7351 149 int TMR2_COUNT_FLOWRATE;
Lightvalve 11:82d8768d7351 150 int TMR2_COUNT_DEADZONE;
Lightvalve 11:82d8768d7351 151 int TMR2_COUNT_PRES_NULL;
Lightvalve 11:82d8768d7351 152 int TMR2_COUNT_TORQUE_NULL;
Lightvalve 11:82d8768d7351 153 int TMR2_COUNT_PRES_CALIB;
Lightvalve 11:82d8768d7351 154 int TMR2_COUNT_REFERENCE;
Lightvalve 11:82d8768d7351 155 int TMR2_COUNT_JOINT;
Lightvalve 11:82d8768d7351 156 int TMR2_COUNT_ROTARY_FRIC_TUNE;
Lightvalve 11:82d8768d7351 157
Lightvalve 11:82d8768d7351 158 bool FLAG_REFERENCE_VALVE_PWM;
Lightvalve 11:82d8768d7351 159 bool FLAG_REFERENCE_VALVE_POSITION;
Lightvalve 11:82d8768d7351 160 bool FLAG_REFERENCE_JOINT_POSITION;
Lightvalve 11:82d8768d7351 161 bool FLAG_REFERENCE_JOINT_TORQUE;
Lightvalve 11:82d8768d7351 162 bool FLAG_REFERENCE_PRES_DIFF;
Lightvalve 11:82d8768d7351 163
Lightvalve 11:82d8768d7351 164 double TUNING_TIME;
Lightvalve 11:82d8768d7351 165
Lightvalve 11:82d8768d7351 166 double REFERENCE_FREQ;
Lightvalve 11:82d8768d7351 167 double REFERENCE_MAG;
Lightvalve 11:82d8768d7351 168
Lightvalve 11:82d8768d7351 169 bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 170
Lightvalve 11:82d8768d7351 171 int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 172 enum _JUMP_STATUS {
Lightvalve 11:82d8768d7351 173 JUMP_NO_ACT = 0, //0
Lightvalve 11:82d8768d7351 174 JUMP_START, //1
Lightvalve 11:82d8768d7351 175 JUMP_TAKEOFF, //2
Lightvalve 11:82d8768d7351 176 JUMP_FLYING, //3
Lightvalve 11:82d8768d7351 177 JUMP_LANDING, //4
Lightvalve 11:82d8768d7351 178 };
Lightvalve 11:82d8768d7351 179
Lightvalve 11:82d8768d7351 180
Lightvalve 11:82d8768d7351 181
Lightvalve 11:82d8768d7351 182
Lightvalve 11:82d8768d7351 183 /*******************************************************************************
Lightvalve 11:82d8768d7351 184 * General math functions
Lightvalve 11:82d8768d7351 185 ******************************************************************************/
Lightvalve 11:82d8768d7351 186
Lightvalve 11:82d8768d7351 187
Lightvalve 11:82d8768d7351 188 double dabs(double tx) {
Lightvalve 11:82d8768d7351 189 if (tx >= 0.0)
Lightvalve 11:82d8768d7351 190 return tx;
Lightvalve 11:82d8768d7351 191 else
Lightvalve 11:82d8768d7351 192 return -tx;
Lightvalve 11:82d8768d7351 193 }
Lightvalve 11:82d8768d7351 194
Lightvalve 11:82d8768d7351 195 double change_int_to_efloat(int input) {
Lightvalve 11:82d8768d7351 196 int i = 0;
Lightvalve 11:82d8768d7351 197
Lightvalve 11:82d8768d7351 198 double output = 0;
Lightvalve 11:82d8768d7351 199 int vn = (int) ((double) input / 10.);
Lightvalve 11:82d8768d7351 200 int en = input % 10;
Lightvalve 11:82d8768d7351 201
Lightvalve 11:82d8768d7351 202 double temp = 1.;
Lightvalve 11:82d8768d7351 203 for (i = 0; i < en; i++)
Lightvalve 11:82d8768d7351 204 temp *= 0.1;
Lightvalve 11:82d8768d7351 205
Lightvalve 11:82d8768d7351 206 output = (double) vn*temp;
Lightvalve 11:82d8768d7351 207 return output;
Lightvalve 11:82d8768d7351 208 }
Lightvalve 11:82d8768d7351 209
Lightvalve 11:82d8768d7351 210 void make_delay(void) {
Lightvalve 11:82d8768d7351 211 int i = 0;
Lightvalve 11:82d8768d7351 212
Lightvalve 11:82d8768d7351 213 for (i = 0; i < 100000; i++) {
Lightvalve 11:82d8768d7351 214 ;
Lightvalve 11:82d8768d7351 215 }
Lightvalve 11:82d8768d7351 216
Lightvalve 11:82d8768d7351 217 }
Lightvalve 11:82d8768d7351 218
Lightvalve 11:82d8768d7351 219
Lightvalve 11:82d8768d7351 220 /*******************************************************************************
Lightvalve 11:82d8768d7351 221 * ROM functions
Lightvalve 11:82d8768d7351 222 ******************************************************************************/
Lightvalve 11:82d8768d7351 223 void ROM_INIT_DATA(void) {
Lightvalve 11:82d8768d7351 224 static int16_t temp_check_num = LATEST_VERSION;
Lightvalve 11:82d8768d7351 225 spi_eeprom_ready();
Lightvalve 11:82d8768d7351 226 if (spi_eeprom_read(RID_IS_FIRST) != temp_check_num) {
Lightvalve 11:82d8768d7351 227 spi_eeprom_ready();
Lightvalve 11:82d8768d7351 228 spi_eeprom_write(RID_IS_FIRST, temp_check_num);
Lightvalve 11:82d8768d7351 229 ROM_RESET_DATA();
Lightvalve 11:82d8768d7351 230 }
Lightvalve 11:82d8768d7351 231 ROM_CALL_DATA();
Lightvalve 11:82d8768d7351 232 }
Lightvalve 11:82d8768d7351 233
Lightvalve 11:82d8768d7351 234 void ROM_RESET_DATA(void) {
Lightvalve 11:82d8768d7351 235
Lightvalve 11:82d8768d7351 236 spi_eeprom_write(RID_BNO, (int16_t) BNO);
Lightvalve 11:82d8768d7351 237 spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
Lightvalve 11:82d8768d7351 238 spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
Lightvalve 11:82d8768d7351 239 spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
Lightvalve 11:82d8768d7351 240 spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
Lightvalve 11:82d8768d7351 241 spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
Lightvalve 11:82d8768d7351 242 spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.));
Lightvalve 11:82d8768d7351 243 spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.));
Lightvalve 11:82d8768d7351 244 spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 245 spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 246 spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 247 spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 248 spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 249 spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 250 spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 251 spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 252 spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 253 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
Lightvalve 11:82d8768d7351 254 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
Lightvalve 11:82d8768d7351 255 spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
Lightvalve 11:82d8768d7351 256 spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
Lightvalve 11:82d8768d7351 257 spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
Lightvalve 11:82d8768d7351 258 spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
Lightvalve 11:82d8768d7351 259 spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
Lightvalve 11:82d8768d7351 260 spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
Lightvalve 11:82d8768d7351 261 spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
Lightvalve 11:82d8768d7351 262 spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
Lightvalve 11:82d8768d7351 263 spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
Lightvalve 11:82d8768d7351 264 spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
Lightvalve 11:82d8768d7351 265 spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
Lightvalve 11:82d8768d7351 266 spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
Lightvalve 11:82d8768d7351 267 spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 268 spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
Lightvalve 11:82d8768d7351 269 spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
Lightvalve 11:82d8768d7351 270 spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 271 spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) ENC_PULSE_PER_POSITION);
Lightvalve 11:82d8768d7351 272 spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) TORQUE_SENSOR_PULSE_PER_TORQUE);
Lightvalve 11:82d8768d7351 273 spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.));
Lightvalve 11:82d8768d7351 274 spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.));
Lightvalve 11:82d8768d7351 275 spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.));
Lightvalve 11:82d8768d7351 276 spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
Lightvalve 11:82d8768d7351 277 spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
Lightvalve 11:82d8768d7351 278 spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.));
Lightvalve 11:82d8768d7351 279 spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.));
Lightvalve 11:82d8768d7351 280 spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.));
Lightvalve 11:82d8768d7351 281 spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.));
Lightvalve 11:82d8768d7351 282 spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.));
Lightvalve 11:82d8768d7351 283 spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.));
Lightvalve 11:82d8768d7351 284 spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.));
Lightvalve 11:82d8768d7351 285 spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.));
Lightvalve 11:82d8768d7351 286 spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.));
Lightvalve 11:82d8768d7351 287 spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.));
Lightvalve 11:82d8768d7351 288 spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.));
Lightvalve 11:82d8768d7351 289 spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.));
Lightvalve 11:82d8768d7351 290 spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.));
Lightvalve 11:82d8768d7351 291
Lightvalve 11:82d8768d7351 292
Lightvalve 11:82d8768d7351 293 }
Lightvalve 11:82d8768d7351 294
Lightvalve 11:82d8768d7351 295 void ROM_CALL_DATA(void) {
Lightvalve 11:82d8768d7351 296 int i = 0;
Lightvalve 11:82d8768d7351 297 BNO = spi_eeprom_read(RID_BNO);
Lightvalve 11:82d8768d7351 298 BNO = 10;
Lightvalve 11:82d8768d7351 299 OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
Lightvalve 11:82d8768d7351 300 CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
Lightvalve 11:82d8768d7351 301
Lightvalve 11:82d8768d7351 302 DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
Lightvalve 11:82d8768d7351 303 DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
Lightvalve 11:82d8768d7351 304 DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
Lightvalve 11:82d8768d7351 305
Lightvalve 11:82d8768d7351 306 SUPPLY_VOLTAGE = (double) spi_eeprom_read(RID_VOLATGE_SUPPLY) / 10.;
Lightvalve 11:82d8768d7351 307 VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.;
Lightvalve 11:82d8768d7351 308
Lightvalve 11:82d8768d7351 309 P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 310 I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 311 D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 312
Lightvalve 11:82d8768d7351 313 P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 314 I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 315 D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
Lightvalve 11:82d8768d7351 316
Lightvalve 11:82d8768d7351 317 P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 318 I_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_I_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 319 D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
Lightvalve 11:82d8768d7351 320
Lightvalve 11:82d8768d7351 321 VALVE_DEADZONE_PLUS = spi_eeprom_read(RID_VALVE_DEADZONE_PLUS);
Lightvalve 11:82d8768d7351 322 VALVE_DEADZONE_MINUS = spi_eeprom_read(RID_VALVE_DEADZONE_MINUS);
Lightvalve 11:82d8768d7351 323
Lightvalve 11:82d8768d7351 324 VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
Lightvalve 11:82d8768d7351 325 COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
Lightvalve 11:82d8768d7351 326
Lightvalve 11:82d8768d7351 327 VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
Lightvalve 11:82d8768d7351 328 VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
Lightvalve 11:82d8768d7351 329 BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
Lightvalve 11:82d8768d7351 330
Lightvalve 11:82d8768d7351 331 CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
Lightvalve 11:82d8768d7351 332 CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
Lightvalve 11:82d8768d7351 333
Lightvalve 11:82d8768d7351 334 PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
Lightvalve 11:82d8768d7351 335 PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
Lightvalve 11:82d8768d7351 336 PISTON_AREA_alpha = (double)PISTON_AREA_B/(double)PISTON_AREA_A;
Lightvalve 11:82d8768d7351 337
Lightvalve 11:82d8768d7351 338 PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
Lightvalve 11:82d8768d7351 339 PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
Lightvalve 11:82d8768d7351 340
Lightvalve 11:82d8768d7351 341 ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
Lightvalve 11:82d8768d7351 342 ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 343
Lightvalve 11:82d8768d7351 344 STROKE = spi_eeprom_read(RID_STROKE);
Lightvalve 11:82d8768d7351 345
Lightvalve 11:82d8768d7351 346 VALVE_LIMIT_MINUS = spi_eeprom_read(RID_VALVE_LIMIT_MINUS);
Lightvalve 11:82d8768d7351 347 VALVE_LIMIT_PLUS = spi_eeprom_read(RID_VALVE_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 348
Lightvalve 11:82d8768d7351 349 ENC_PULSE_PER_POSITION = spi_eeprom_read(RID_ENC_PULSE_PER_POSITION);
Lightvalve 11:82d8768d7351 350 TORQUE_SENSOR_PULSE_PER_TORQUE = spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
Lightvalve 11:82d8768d7351 351 PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
Lightvalve 11:82d8768d7351 352 PRES_SENSOR_B_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR) * 0.01;
Lightvalve 11:82d8768d7351 353
Lightvalve 11:82d8768d7351 354 FRICTION = (double) spi_eeprom_read(RID_FRICTION) / 10.;
Lightvalve 11:82d8768d7351 355 HOMEPOS_OFFSET = (int) spi_eeprom_read(RID_HOMEPOS_OFFSET);
Lightvalve 11:82d8768d7351 356 HOMEPOS_VALVE_OPENING = (int) spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
Lightvalve 11:82d8768d7351 357
Lightvalve 11:82d8768d7351 358 TORQUE_VREF = (double) spi_eeprom_read(RID_TORQUE_SENSOR_VREF) / 1000.;
Lightvalve 11:82d8768d7351 359
Lightvalve 11:82d8768d7351 360 PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.;
Lightvalve 11:82d8768d7351 361 PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.;
Lightvalve 11:82d8768d7351 362
Lightvalve 11:82d8768d7351 363 VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.;
Lightvalve 11:82d8768d7351 364 VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.;
Lightvalve 11:82d8768d7351 365 VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.;
Lightvalve 11:82d8768d7351 366 VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.;
Lightvalve 11:82d8768d7351 367 VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.;
Lightvalve 11:82d8768d7351 368 VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.;
Lightvalve 11:82d8768d7351 369 VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.;
Lightvalve 11:82d8768d7351 370 VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.;
Lightvalve 11:82d8768d7351 371 VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.;
Lightvalve 11:82d8768d7351 372 VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.;
Lightvalve 11:82d8768d7351 373
Lightvalve 11:82d8768d7351 374 for(i=0; i<18; i++)
Lightvalve 11:82d8768d7351 375 {
Lightvalve 11:82d8768d7351 376 VALVE_POS_VS_PWM[i] = (double) spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i);
Lightvalve 11:82d8768d7351 377 }
Lightvalve 11:82d8768d7351 378 for(i=0; i<100; i++)
Lightvalve 11:82d8768d7351 379 {
Lightvalve 12:6f2531038ea4 380 JOINT_VEL[i] = (int32_t) ( (spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0 + i) & 0xFFFF) | ((spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0_1 + i) & 0xFFFF) << 16));
Lightvalve 11:82d8768d7351 381 }
Lightvalve 11:82d8768d7351 382
Lightvalve 11:82d8768d7351 383 VALVE_MAX_POS = (int) spi_eeprom_read(RID_VALVE_MAX_POS);
Lightvalve 11:82d8768d7351 384 VALVE_MIN_POS = (int) spi_eeprom_read(RID_VALVE_MIN_POS);
Lightvalve 11:82d8768d7351 385 DDV_CENTER = (int) spi_eeprom_read(RID_DDV_CNETER);
Lightvalve 11:82d8768d7351 386
Lightvalve 11:82d8768d7351 387 VALVE_POS_NUM = (int) spi_eeprom_read(RID_VALVE_POS_NUM);
Lightvalve 11:82d8768d7351 388
Lightvalve 11:82d8768d7351 389 }
Lightvalve 11:82d8768d7351 390
Lightvalve 11:82d8768d7351 391 /*******************************************************************************
Lightvalve 11:82d8768d7351 392 * ENCODER functions
Lightvalve 11:82d8768d7351 393 ******************************************************************************/
Lightvalve 11:82d8768d7351 394 // A-KHA
Lightvalve 11:82d8768d7351 395 #define KF_G1_11 0.083920206005350
Lightvalve 11:82d8768d7351 396 #define KF_G1_12 0.000013905329560
Lightvalve 11:82d8768d7351 397 #define KF_G1_21 -0.000575742328210
Lightvalve 11:82d8768d7351 398 #define KF_G1_22 0.799999939711725
Lightvalve 11:82d8768d7351 399 // K
Lightvalve 11:82d8768d7351 400 #define KF_G2_11 0.916079793994650
Lightvalve 11:82d8768d7351 401 #define KF_G2_12 0.000002878711641
Lightvalve 11:82d8768d7351 402 #define KF_G2_21 0.000575742328210
Lightvalve 11:82d8768d7351 403 #define KF_G2_22 0.199999945139809
Lightvalve 11:82d8768d7351 404
Lightvalve 11:82d8768d7351 405 double KF_Y_11 = 0.;
Lightvalve 11:82d8768d7351 406 double KF_Y_21 = 0.;
Lightvalve 11:82d8768d7351 407 double KF_X_11 = 0.;
Lightvalve 11:82d8768d7351 408 double KF_X_21 = 0.;
Lightvalve 11:82d8768d7351 409
Lightvalve 11:82d8768d7351 410 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
Lightvalve 11:82d8768d7351 411 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
Lightvalve 11:82d8768d7351 412 long enc_offset = 0;
Lightvalve 11:82d8768d7351 413
Lightvalve 11:82d8768d7351 414 void ENC_UPDATE(void) {
Lightvalve 11:82d8768d7351 415
Lightvalve 11:82d8768d7351 416 ENC_pos_cur = spi_enc_read();
Lightvalve 11:82d8768d7351 417 ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
Lightvalve 11:82d8768d7351 418
Lightvalve 11:82d8768d7351 419 if (ENC_pos_diff > 1300 || ENC_pos_diff<-1300) {
Lightvalve 11:82d8768d7351 420 //MOT_E_STOP(0);
Lightvalve 11:82d8768d7351 421 }
Lightvalve 11:82d8768d7351 422
Lightvalve 12:6f2531038ea4 423 ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
Lightvalve 11:82d8768d7351 424
Lightvalve 11:82d8768d7351 425 KF_Y_11 = ENC_pos_cur;
Lightvalve 11:82d8768d7351 426 KF_Y_21 = ENC_VEL_RAW;
Lightvalve 11:82d8768d7351 427 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 428 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 429 ENC_VEL_KF = (int32_t) KF_X_21;
Lightvalve 11:82d8768d7351 430
Lightvalve 11:82d8768d7351 431 CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
Lightvalve 11:82d8768d7351 432 // CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
Lightvalve 11:82d8768d7351 433 CUR_VELOCITY = (DIR_JOINT_ENC) * ENC_VEL_KF;
Lightvalve 11:82d8768d7351 434
Lightvalve 11:82d8768d7351 435 // CUR_POSITION = ENC_pos_cur;
Lightvalve 11:82d8768d7351 436 // CUR_VELOCITY = ENC_VEL_KF;
Lightvalve 11:82d8768d7351 437
Lightvalve 11:82d8768d7351 438 ENC_pos_old = ENC_pos_cur;
Lightvalve 11:82d8768d7351 439 }
Lightvalve 11:82d8768d7351 440
Lightvalve 11:82d8768d7351 441 void ENC_SET_ZERO(void) {
Lightvalve 11:82d8768d7351 442
Lightvalve 11:82d8768d7351 443 spi_enc_set_clear();
Lightvalve 11:82d8768d7351 444 CUR_POSITION = 0;
Lightvalve 11:82d8768d7351 445 ENC_pos_old = ENC_pos_cur = 0;
Lightvalve 11:82d8768d7351 446
Lightvalve 11:82d8768d7351 447 }
Lightvalve 11:82d8768d7351 448
Lightvalve 12:6f2531038ea4 449 void ENC_SET(int32_t value) {
Lightvalve 11:82d8768d7351 450
Lightvalve 11:82d8768d7351 451 spi_enc_set_clear();
Lightvalve 11:82d8768d7351 452 enc_offset = value;
Lightvalve 11:82d8768d7351 453 CUR_POSITION = value;
Lightvalve 11:82d8768d7351 454 ENC_pos_old = ENC_pos_cur = value;
Lightvalve 11:82d8768d7351 455
Lightvalve 11:82d8768d7351 456 }