Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Learning

for learning

Dependencies:   mbed FastPWM

setting.h@249:21430e06f706, 2021-01-08 (annotated)

Committer:
Lightvalve
Date:
Fri Jan 08 04:37:14 2021 +0000
Revision:
249:21430e06f706
Parent:
248:c925c863ea87
Child:
250:ae6e0c80029d
210108_3  500Hz   num_input 9    210105 data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GiJeongKim 0:51c43836c1d7 1 #include "mbed.h"
GiJeongKim 0:51c43836c1d7 2 #include "FastPWM.h"
GiJeongKim 0:51c43836c1d7 3
GiJeongKim 0:51c43836c1d7 4 // pwm
jobuuu 2:a1c0a37df760 5 #define PIN_V PB_7
jobuuu 2:a1c0a37df760 6 #define PIN_W PB_6
jobuuu 2:a1c0a37df760 7 //#define PWM_ARR 0x465 // loop 80k, pwm 40k
jobuuu 2:a1c0a37df760 8 //#define PWM_ARR 0x8CA // loop 40k, pwm 20k
Lightvalve 51:b46bed7fec80 9 #define PWM_ARR 0x1194 // loop 20k, pwm 10k
Lightvalve 51:b46bed7fec80 10 //#define PWM_ARR 0x2328 // loop 10k, pwm 5k
Lightvalve 11:82d8768d7351 11 #define TMR3_COUNT 0x4650 // loop 5k
Lightvalve 23:59218d4a256d 12 #define TMR2_COUNT 0x2710 // loop 500hz with prescale 18
jobuuu 2:a1c0a37df760 13
Lightvalve 30:8d561f16383b 14 #define FREQ_500 500.0f
Lightvalve 30:8d561f16383b 15 #define FREQ_1k 1000.0f
Lightvalve 30:8d561f16383b 16 #define FREQ_5k 5000.0f
Lightvalve 30:8d561f16383b 17 #define FREQ_10k 10000.0f
Lightvalve 30:8d561f16383b 18 #define FREQ_20k 20000.0f
Lightvalve 30:8d561f16383b 19 #define FREQ_40k 40000.0f
Lightvalve 30:8d561f16383b 20 #define DT_500 0.002f
Lightvalve 30:8d561f16383b 21 #define DT_1k 0.0005f
Lightvalve 30:8d561f16383b 22 #define DT_5k 0.0002f
Lightvalve 30:8d561f16383b 23 #define DT_10k 0.0001f
Lightvalve 30:8d561f16383b 24 #define DT_20k 0.00005f
Lightvalve 30:8d561f16383b 25 #define DT_40k 0.000025f
jobuuu 5:a4319f79457b 26
jobuuu 7:e9086c72bb22 27 extern DigitalOut check;
jobuuu 7:e9086c72bb22 28 extern DigitalOut check_2;
jobuuu 7:e9086c72bb22 29 extern AnalogOut dac_1;
jobuuu 7:e9086c72bb22 30 extern AnalogOut dac_2;
jobuuu 7:e9086c72bb22 31
Lightvalve 30:8d561f16383b 32 extern float dtc_v;
Lightvalve 30:8d561f16383b 33 extern float dtc_w;
GiJeongKim 0:51c43836c1d7 34
GiJeongKim 0:51c43836c1d7 35 // I2C
GiJeongKim 0:51c43836c1d7 36 extern I2C i2c; // SDA, SCL (for K22F)
GiJeongKim 0:51c43836c1d7 37 extern const int i2c_slave_addr1;
GiJeongKim 0:51c43836c1d7 38 extern unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim 0:51c43836c1d7 39
GiJeongKim 0:51c43836c1d7 40 // SPI
Lightvalve 170:42c938a40313 41 extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK);
Lightvalve 170:42c938a40313 42 extern DigitalOut eeprom_cs;
GiJeongKim 0:51c43836c1d7 43 extern SPI enc;
GiJeongKim 0:51c43836c1d7 44 extern DigitalOut enc_cs;
GiJeongKim 0:51c43836c1d7 45
GiJeongKim 0:51c43836c1d7 46 // UART
jobuuu 7:e9086c72bb22 47 extern Serial pc; //Serial pc(PA_9,PA_10); _ UART
jobuuu 7:e9086c72bb22 48
jobuuu 7:e9086c72bb22 49 // CAN
jobuuu 7:e9086c72bb22 50 extern CAN can;
jobuuu 7:e9086c72bb22 51 extern CANMessage msg;
jobuuu 7:e9086c72bb22 52
jobuuu 7:e9086c72bb22 53 // Board Information
jobuuu 7:e9086c72bb22 54
jobuuu 7:e9086c72bb22 55
jobuuu 7:e9086c72bb22 56 // User Function
jobuuu 7:e9086c72bb22 57
jobuuu 7:e9086c72bb22 58 void CurrentControl();
jobuuu 7:e9086c72bb22 59 void ValveControl(unsigned int ControlMode);
Lightvalve 11:82d8768d7351 60
Lightvalve 11:82d8768d7351 61
Lightvalve 11:82d8768d7351 62 /*******************************************************************************
Lightvalve 11:82d8768d7351 63 * COMMON Settings
Lightvalve 11:82d8768d7351 64 ******************************************************************************/
Lightvalve 11:82d8768d7351 65 #define LATEST_VERSION 19032
Lightvalve 11:82d8768d7351 66
Lightvalve 11:82d8768d7351 67
Lightvalve 11:82d8768d7351 68
Lightvalve 11:82d8768d7351 69 /*******************************************************************************
Lightvalve 11:82d8768d7351 70 * COMMON CONSTANTS
Lightvalve 11:82d8768d7351 71 ******************************************************************************/
Lightvalve 30:8d561f16383b 72 #define RAD_30 0.523598775598299f
Lightvalve 30:8d561f16383b 73 #define RAD_60 1.047197551196598f
Lightvalve 30:8d561f16383b 74 #define RAD_120 2.094395102393195f
Lightvalve 30:8d561f16383b 75 #define RAD_180 3.141592653589793f
Lightvalve 30:8d561f16383b 76 #define RAD_240 4.188790204786391f
Lightvalve 30:8d561f16383b 77 #define RAD_300 5.235987755982989f
Lightvalve 30:8d561f16383b 78 #define RAD_360 6.283185307179586f
Lightvalve 11:82d8768d7351 79
Lightvalve 11:82d8768d7351 80 #define SYSFREQ 200000000
Lightvalve 11:82d8768d7351 81 #define PBCLK 100000000
Lightvalve 11:82d8768d7351 82
Lightvalve 11:82d8768d7351 83 //#define TMR_FREQ_10k 10000
Lightvalve 11:82d8768d7351 84 #define TMR_FREQ_5k 5000
Lightvalve 11:82d8768d7351 85
Lightvalve 11:82d8768d7351 86 #define FALSE 0
Lightvalve 11:82d8768d7351 87 #define TRUE 1
Lightvalve 11:82d8768d7351 88 #define OUTPUT 0
Lightvalve 11:82d8768d7351 89 #define INPUT 1
Lightvalve 11:82d8768d7351 90 #define LOW 0
Lightvalve 11:82d8768d7351 91 #define HIGH 1
Lightvalve 11:82d8768d7351 92 #define DIGITAL 0
Lightvalve 11:82d8768d7351 93 #define ANALOG 1
Lightvalve 11:82d8768d7351 94
Lightvalve 30:8d561f16383b 95 #define PI 3.141592653589793f
Lightvalve 30:8d561f16383b 96 #define D2R 0.017453292519943f
Lightvalve 30:8d561f16383b 97 #define R2D 57.295779513082323f
Lightvalve 11:82d8768d7351 98
Lightvalve 11:82d8768d7351 99
Lightvalve 11:82d8768d7351 100 /*******************************************************************************
Lightvalve 11:82d8768d7351 101 * HEADER INCLUDE
Lightvalve 11:82d8768d7351 102 ******************************************************************************/
Lightvalve 11:82d8768d7351 103
Lightvalve 11:82d8768d7351 104 /*******************************************************************************
Lightvalve 11:82d8768d7351 105 * VARIABLE
Lightvalve 11:82d8768d7351 106 ******************************************************************************/
Lightvalve 11:82d8768d7351 107
Lightvalve 11:82d8768d7351 108 // Board Information
Lightvalve 11:82d8768d7351 109 extern uint8_t BNO;
Lightvalve 11:82d8768d7351 110 extern uint8_t CONTROL_MODE;
Lightvalve 11:82d8768d7351 111 extern uint8_t OPERATING_MODE;
Lightvalve 52:8ea76864368a 112 extern uint8_t SENSING_MODE;
Lightvalve 57:f4819de54e7a 113 extern uint8_t CONTROL_UTILITY_MODE;
Lightvalve 52:8ea76864368a 114 extern uint8_t CURRENT_CONTROL_MODE;
Lightvalve 52:8ea76864368a 115 extern uint8_t FLAG_VALVE_DEADZONE;
Lightvalve 11:82d8768d7351 116 extern uint8_t REFERENCE_MODE;
Lightvalve 54:647072f5307a 117 extern int16_t CAN_FREQ;
Lightvalve 17:1865016ca2e7 118 extern int16_t DIR_JOINT_ENC;
Lightvalve 17:1865016ca2e7 119 extern int16_t DIR_VALVE;
Lightvalve 17:1865016ca2e7 120 extern int16_t DIR_VALVE_ENC;
Lightvalve 11:82d8768d7351 121
Lightvalve 30:8d561f16383b 122 extern float SUPPLY_VOLTAGE;
Lightvalve 30:8d561f16383b 123 extern float VALVE_VOLTAGE_LIMIT;
Lightvalve 11:82d8768d7351 124
Lightvalve 30:8d561f16383b 125 extern float P_GAIN_VALVE_POSITION;
Lightvalve 30:8d561f16383b 126 extern float I_GAIN_VALVE_POSITION;
Lightvalve 30:8d561f16383b 127 extern float D_GAIN_VALVE_POSITION;
Lightvalve 30:8d561f16383b 128 extern float P_GAIN_JOINT_POSITION;
Lightvalve 30:8d561f16383b 129 extern float I_GAIN_JOINT_POSITION;
Lightvalve 30:8d561f16383b 130 extern float D_GAIN_JOINT_POSITION;
Lightvalve 30:8d561f16383b 131 extern float P_GAIN_JOINT_TORQUE;
Lightvalve 30:8d561f16383b 132 extern float I_GAIN_JOINT_TORQUE;
Lightvalve 30:8d561f16383b 133 extern float D_GAIN_JOINT_TORQUE;
Lightvalve 72:3436ce769b1e 134 extern float P_GAIN_JOINT_TORQUE_FF;
Lightvalve 72:3436ce769b1e 135 extern float I_GAIN_JOINT_TORQUE_FF;
Lightvalve 72:3436ce769b1e 136 extern float D_GAIN_JOINT_TORQUE_FF;
Lightvalve 11:82d8768d7351 137
Lightvalve 46:2694daea349b 138 extern int16_t K_SPRING;
Lightvalve 46:2694daea349b 139 extern int16_t D_DAMPER;
Lightvalve 46:2694daea349b 140
Lightvalve 54:647072f5307a 141 extern int16_t flag_delay_test;
Lightvalve 54:647072f5307a 142
Lightvalve 36:a46e63505ed8 143 //extern float P_GAIN_VALVE_POSITION_OPP;
Lightvalve 36:a46e63505ed8 144 //extern float I_GAIN_VALVE_POSITION_OPP;
Lightvalve 36:a46e63505ed8 145 //extern float D_GAIN_VALVE_POSITION_OPP;
Lightvalve 36:a46e63505ed8 146 //extern float P_GAIN_JOINT_POSITION_OPP;
Lightvalve 36:a46e63505ed8 147 //extern float I_GAIN_JOINT_POSITION_OPP;
Lightvalve 36:a46e63505ed8 148 //extern float D_GAIN_JOINT_POSITION_OPP;
Lightvalve 36:a46e63505ed8 149 //extern float P_GAIN_JOINT_TORQUE_OPP;
Lightvalve 36:a46e63505ed8 150 //extern float I_GAIN_JOINT_TORQUE_OPP;
Lightvalve 36:a46e63505ed8 151 //extern float D_GAIN_JOINT_TORQUE_OPP;
Lightvalve 36:a46e63505ed8 152
Lightvalve 33:91b17819ec30 153 extern float VALVE_DEADZONE_PLUS;
Lightvalve 33:91b17819ec30 154 extern float VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 155
Lightvalve 11:82d8768d7351 156 extern int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 157 extern int16_t COMPLIANCE_GAIN;
Lightvalve 11:82d8768d7351 158
Lightvalve 11:82d8768d7351 159 extern int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 160
Lightvalve 11:82d8768d7351 161 extern int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 162
Lightvalve 11:82d8768d7351 163 extern int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 164
Lightvalve 11:82d8768d7351 165 extern int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 166 extern int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 167
Lightvalve 11:82d8768d7351 168 extern int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 169 extern int16_t PISTON_AREA_B;
Lightvalve 30:8d561f16383b 170 extern float PISTON_AREA_alpha;
Lightvalve 57:f4819de54e7a 171 extern float alpha3;
Lightvalve 11:82d8768d7351 172
Lightvalve 11:82d8768d7351 173
Lightvalve 11:82d8768d7351 174 extern int16_t PRES_SUPPLY;
Lightvalve 11:82d8768d7351 175 extern int16_t PRES_RETURN;
Lightvalve 11:82d8768d7351 176
Lightvalve 11:82d8768d7351 177 extern int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 178 extern int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 179
Lightvalve 11:82d8768d7351 180 extern int16_t STROKE;
Lightvalve 11:82d8768d7351 181
Lightvalve 11:82d8768d7351 182
Lightvalve 133:22ab22818e01 183 extern float Amm;
Lightvalve 133:22ab22818e01 184 extern float beta;
Lightvalve 133:22ab22818e01 185 extern float Ps;
Lightvalve 133:22ab22818e01 186 extern float Pt;
Lightvalve 133:22ab22818e01 187 extern float gamma_hat;
Lightvalve 218:066030f7951f 188 extern float a_hat;
Lightvalve 170:42c938a40313 189 extern float V_adapt;
Lightvalve 133:22ab22818e01 190 extern float x_4_des_old;
Lightvalve 133:22ab22818e01 191
Lightvalve 57:f4819de54e7a 192 //extern int16_t VALVE_LIMIT_PLUS;
Lightvalve 57:f4819de54e7a 193 //extern int16_t VALVE_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 194
Lightvalve 48:889798ff9329 195 extern float ENC_PULSE_PER_POSITION;
Lightvalve 48:889798ff9329 196 extern float TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 48:889798ff9329 197
Lightvalve 30:8d561f16383b 198 extern float PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve 30:8d561f16383b 199 extern float PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve 11:82d8768d7351 200
Lightvalve 11:82d8768d7351 201 extern int HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 202 extern int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 203
Lightvalve 30:8d561f16383b 204 extern float FRICTION;
Lightvalve 30:8d561f16383b 205 extern float REF_PERIOD;
Lightvalve 30:8d561f16383b 206 extern float REF_MAG;
Lightvalve 11:82d8768d7351 207 extern int REF_NUM;
Lightvalve 11:82d8768d7351 208
Lightvalve 11:82d8768d7351 209
Lightvalve 30:8d561f16383b 210 extern float DAC_REF;
Lightvalve 30:8d561f16383b 211 extern float DAC_RESOL;
Lightvalve 11:82d8768d7351 212
Lightvalve 11:82d8768d7351 213 extern int REF_POSITION;
Lightvalve 11:82d8768d7351 214 extern int REF_VELOCITY;
Lightvalve 11:82d8768d7351 215 extern int16_t REF_TORQUE;
Lightvalve 11:82d8768d7351 216 extern int16_t REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 217 extern int16_t REF_PWM;
Lightvalve 11:82d8768d7351 218 extern int16_t REF_VALVE_POSITION;
Lightvalve 14:8e7590227d22 219 extern int16_t REF_CURRENT;
Lightvalve 11:82d8768d7351 220
Lightvalve 11:82d8768d7351 221 extern int REF_MOVE_TIME_5k;
Lightvalve 11:82d8768d7351 222 extern int INIT_REF_PWM;
Lightvalve 11:82d8768d7351 223 extern int INIT_REF_VALVE_POS;
Lightvalve 11:82d8768d7351 224 extern int INIT_REF_POS;
Lightvalve 11:82d8768d7351 225 extern int INIT_REF_VEL;
Lightvalve 11:82d8768d7351 226 extern int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 227 extern int INIT_REF_PRES_DIFF;
Lightvalve 14:8e7590227d22 228 extern int INIT_REF_CURRENT;
Lightvalve 11:82d8768d7351 229
Lightvalve 11:82d8768d7351 230 extern int CUR_POSITION;
Lightvalve 11:82d8768d7351 231 extern int CUR_VELOCITY;
Lightvalve 30:8d561f16383b 232 extern float CUR_TORQUE;
Lightvalve 30:8d561f16383b 233 extern float CUR_PRES_A;
Lightvalve 30:8d561f16383b 234 extern float CUR_PRES_B;
Lightvalve 11:82d8768d7351 235 extern int CUR_VALVE_POSITION;
Lightvalve 11:82d8768d7351 236
Lightvalve 11:82d8768d7351 237 extern unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 238 extern unsigned int TMR2_COUNT_LED2;
Lightvalve 11:82d8768d7351 239 extern unsigned int TMR2_COUNT_CAN_TX;
Lightvalve 54:647072f5307a 240 extern unsigned int TMR3_COUNT_TEST;
Lightvalve 11:82d8768d7351 241
Lightvalve 11:82d8768d7351 242 extern int num_err;
Lightvalve 11:82d8768d7351 243 extern int flag_err[8];
Lightvalve 11:82d8768d7351 244 extern int flag_err_old[8];
Lightvalve 11:82d8768d7351 245 extern int flag_err_rt;
Lightvalve 11:82d8768d7351 246
Lightvalve 11:82d8768d7351 247 extern int flag_ref_enable;
Lightvalve 11:82d8768d7351 248
Lightvalve 11:82d8768d7351 249 extern int flag_data_request[5];
Lightvalve 11:82d8768d7351 250
Lightvalve 45:35fa6884d0c6 251 extern int MODE_POS_FT_TRANS;
Lightvalve 66:a8e6799dbce3 252 extern int NN_Control_Flag;
Lightvalve 45:35fa6884d0c6 253
Lightvalve 169:645207e160ca 254 extern int cnt_buffer;
Lightvalve 169:645207e160ca 255
Lightvalve 57:f4819de54e7a 256 extern float CUR_CURRENT_mA;
Lightvalve 30:8d561f16383b 257 extern float CUR_PRES_A_BAR;
Lightvalve 30:8d561f16383b 258 extern float CUR_PRES_B_BAR;
Lightvalve 30:8d561f16383b 259 extern float CUR_TORQUE_NM;
Lightvalve 30:8d561f16383b 260 extern float CUR_TORQUE_NM_PRESS;
Lightvalve 11:82d8768d7351 261
Lightvalve 30:8d561f16383b 262 extern float PRES_A_VREF;
Lightvalve 30:8d561f16383b 263 extern float PRES_B_VREF;
Lightvalve 30:8d561f16383b 264 extern float TORQUE_VREF;
Lightvalve 11:82d8768d7351 265
Lightvalve 30:8d561f16383b 266 extern float VALVE_PWM_RAW_FB;
Lightvalve 30:8d561f16383b 267 extern float VALVE_PWM_RAW_FF;
Lightvalve 57:f4819de54e7a 268 extern float VALVE_PWM_RAW;
Lightvalve 11:82d8768d7351 269 extern int VALVE_PWM_VALVE_DZ;
Lightvalve 11:82d8768d7351 270
Lightvalve 30:8d561f16383b 271 extern float VALVE_GAIN_LPM_PER_V[10];
Lightvalve 30:8d561f16383b 272 extern float VALVE_POS_VS_PWM[25];
Lightvalve 11:82d8768d7351 273 extern long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 274
Lightvalve 11:82d8768d7351 275 extern int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 276 extern int VALVE_MIN_POS;
Lightvalve 11:82d8768d7351 277 extern int VALVE_POS_NUM;
Lightvalve 32:4b8c0fedaf2c 278 extern float VALVE_CENTER_OFFSET;
Lightvalve 33:91b17819ec30 279 extern float VALVE_DZ_MINUS_OFFSET;
Lightvalve 33:91b17819ec30 280 extern float VALVE_DZ_PLUS_OFFSET;
Lightvalve 33:91b17819ec30 281 extern float VALVE_DEADZONE_MINUS;
Lightvalve 33:91b17819ec30 282 extern float VALVE_DEADZONE_PLUS;
Lightvalve 11:82d8768d7351 283
Lightvalve 14:8e7590227d22 284 extern int TMR3_COUNT_FINDHOME;
Lightvalve 14:8e7590227d22 285 extern int TMR3_COUNT_FLOWRATE;
Lightvalve 14:8e7590227d22 286 extern int TMR3_COUNT_DEADZONE;
Lightvalve 14:8e7590227d22 287 extern int TMR3_COUNT_PRES_NULL;
Lightvalve 14:8e7590227d22 288 extern int TMR3_COUNT_TORQUE_NULL;
Lightvalve 14:8e7590227d22 289 extern int TMR3_COUNT_PRES_CALIB;
Lightvalve 14:8e7590227d22 290 extern int TMR3_COUNT_REFERENCE;
Lightvalve 14:8e7590227d22 291 extern int TMR3_COUNT_JOINT;
Lightvalve 14:8e7590227d22 292 extern int TMR3_COUNT_ROTARY_FRIC_TUNE;
Lightvalve 11:82d8768d7351 293
Lightvalve 30:8d561f16383b 294 extern float TUNING_TIME;
Lightvalve 11:82d8768d7351 295
Lightvalve 30:8d561f16383b 296 extern float REFERENCE_FREQ;
Lightvalve 30:8d561f16383b 297 extern float REFERENCE_MAG;
Lightvalve 11:82d8768d7351 298
Lightvalve 11:82d8768d7351 299 extern bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 300
Lightvalve 11:82d8768d7351 301 extern int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 302
Lightvalve 30:8d561f16383b 303 extern float CUR_PRES_DIFF_BAR;
Lightvalve 30:8d561f16383b 304 extern float CUR_PRES_A_sum;
Lightvalve 30:8d561f16383b 305 extern float CUR_PRES_B_sum;
Lightvalve 30:8d561f16383b 306 extern float CUR_PRES_A_mean;
Lightvalve 30:8d561f16383b 307 extern float CUR_PRES_B_mean;
Lightvalve 30:8d561f16383b 308 extern float CUR_TORQUE_sum;
Lightvalve 30:8d561f16383b 309 extern float CUR_TORQUE_mean;
Lightvalve 30:8d561f16383b 310 extern float PRES_A_NULL;
Lightvalve 30:8d561f16383b 311 extern float PRES_B_NULL;
Lightvalve 30:8d561f16383b 312 extern float TORQUE_NULL;
Lightvalve 13:747daba9cf59 313
Lightvalve 30:8d561f16383b 314 extern float Ref_Valve_Pos_Old;
Lightvalve 13:747daba9cf59 315
Lightvalve 13:747daba9cf59 316 extern int VALVE_ID_timer;
Lightvalve 13:747daba9cf59 317 extern int VALVE_DZ_timer;
Lightvalve 13:747daba9cf59 318 extern int VALVE_FR_timer;
Lightvalve 35:34ce7b0347b8 319 extern int VALVE_HPL_timer;
Lightvalve 13:747daba9cf59 320 extern int VALVE_POS_TMP;
Lightvalve 13:747daba9cf59 321 extern int JOINT_VEL_TMP;
Lightvalve 13:747daba9cf59 322 extern int DDV_POS_AVG ;
Lightvalve 19:23b7c1ad8683 323 extern int VALVE_POS_AVG[50];
Lightvalve 13:747daba9cf59 324 extern int VALVE_POS_AVG_OLD;
Lightvalve 13:747daba9cf59 325 extern int data_num;
Lightvalve 13:747daba9cf59 326 extern int ID_index;
Lightvalve 13:747daba9cf59 327 extern int DZ_index;
Lightvalve 19:23b7c1ad8683 328 extern int ID_index_array[50];
Lightvalve 13:747daba9cf59 329 extern int first_check;
Lightvalve 30:8d561f16383b 330 extern float init_time;
Lightvalve 13:747daba9cf59 331 extern int DZ_case;
Lightvalve 13:747daba9cf59 332 extern int START_POS;
Lightvalve 13:747daba9cf59 333 extern int FINAL_POS;
Lightvalve 13:747daba9cf59 334 extern int DZ_DIRECTION;
Lightvalve 13:747daba9cf59 335 extern int FIRST_DZ;
Lightvalve 13:747daba9cf59 336 extern int SECOND_DZ;
Lightvalve 13:747daba9cf59 337 extern int DZ_NUM;
Lightvalve 13:747daba9cf59 338 extern int one_period_end;
Lightvalve 30:8d561f16383b 339 extern float Ref_Vel_Test;
Lightvalve 13:747daba9cf59 340 extern long TMR2_FOR_SLOW_LOGGING;
Lightvalve 19:23b7c1ad8683 341 //extern int velcount;
Lightvalve 13:747daba9cf59 342 extern char max_check;
Lightvalve 13:747daba9cf59 343 extern char min_check;
Lightvalve 13:747daba9cf59 344
Lightvalve 30:8d561f16383b 345 extern float valve_pos_err, valve_pos_err_old, valve_pos_err_diff, valve_pos_err_sum;
Lightvalve 30:8d561f16383b 346 extern float joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_diff_fil, joint_pos_err_sum;
Lightvalve 30:8d561f16383b 347 extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum;
Lightvalve 30:8d561f16383b 348 extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ;
Lightvalve 13:747daba9cf59 349
Lightvalve 30:8d561f16383b 350 extern float CUR_FLOWRATE;
Lightvalve 30:8d561f16383b 351 extern float VALVE_FF_VOLTAGE;
Lightvalve 13:747daba9cf59 352
Lightvalve 13:747daba9cf59 353 extern int pos_plus_end;
Lightvalve 13:747daba9cf59 354 extern int pos_minus_end;
Lightvalve 13:747daba9cf59 355
Lightvalve 13:747daba9cf59 356 extern bool need_enc_init;
Lightvalve 13:747daba9cf59 357
Lightvalve 13:747daba9cf59 358 extern int temp_time;
Lightvalve 13:747daba9cf59 359
Lightvalve 30:8d561f16383b 360 extern float CUR_VELOCITY_sum;
Lightvalve 30:8d561f16383b 361 extern float temp_vel_sum;
Lightvalve 13:747daba9cf59 362
Lightvalve 13:747daba9cf59 363 extern int DZ_dir;
Lightvalve 13:747daba9cf59 364 extern int DZ_temp_cnt;
Lightvalve 13:747daba9cf59 365 extern int DZ_temp_cnt2;
Lightvalve 13:747daba9cf59 366 extern int DZ_end;
Lightvalve 13:747daba9cf59 367 extern int flag_flowrate;
Lightvalve 13:747daba9cf59 368 extern int fl_temp_cnt;
Lightvalve 13:747daba9cf59 369 extern int fl_temp_cnt2;
Lightvalve 13:747daba9cf59 370 extern int cur_vel_sum;
Lightvalve 13:747daba9cf59 371
Lightvalve 57:f4819de54e7a 372 extern float Cur_Valve_Open_pulse;
Lightvalve 57:f4819de54e7a 373
Lightvalve 13:747daba9cf59 374 // find home
Lightvalve 13:747daba9cf59 375 extern int CUR_VELOCITY_OLD;
Lightvalve 13:747daba9cf59 376 extern int cnt_findhome;
Lightvalve 13:747daba9cf59 377 extern int cnt_vel_findhome;
Lightvalve 13:747daba9cf59 378 extern int FINDHOME_VELOCITY;
Lightvalve 13:747daba9cf59 379 extern int FINDHOME_VELOCITY_OLD;
Lightvalve 13:747daba9cf59 380 extern int FINDHOME_POSITION;
Lightvalve 13:747daba9cf59 381 extern int FINDHOME_POSITION_OLD;
Lightvalve 13:747daba9cf59 382
Lightvalve 13:747daba9cf59 383 // valve gain
Lightvalve 13:747daba9cf59 384 extern int check_vel_pos_init;
Lightvalve 13:747daba9cf59 385 extern int check_vel_pos_fin;
Lightvalve 13:747daba9cf59 386 extern int check_vel_pos_interv;
Lightvalve 13:747daba9cf59 387 extern int valve_gain_repeat_cnt;
Lightvalve 30:8d561f16383b 388 extern float VALVE_VOLTAGE;
Lightvalve 13:747daba9cf59 389
Lightvalve 30:8d561f16383b 390 extern float freq_fric_tune;
Lightvalve 13:747daba9cf59 391
Lightvalve 14:8e7590227d22 392 extern uint32_t TMR3_COUNT_CAN_TX;
Lightvalve 14:8e7590227d22 393
Lightvalve 57:f4819de54e7a 394 // Current Control Variables
Lightvalve 57:f4819de54e7a 395 extern double I_REF;
Lightvalve 57:f4819de54e7a 396 extern double I_REF_fil;
Lightvalve 57:f4819de54e7a 397 extern double I_ERR;
Lightvalve 57:f4819de54e7a 398 extern double I_ERR_INT;
Lightvalve 57:f4819de54e7a 399 extern double I_REF_fil_old;
Lightvalve 57:f4819de54e7a 400 extern double I_REF_fil_diff;
Lightvalve 57:f4819de54e7a 401
Lightvalve 57:f4819de54e7a 402 // system id
Lightvalve 57:f4819de54e7a 403 extern int cnt_sysid;
Lightvalve 57:f4819de54e7a 404 extern double freq_sysid_Iref;
Lightvalve 57:f4819de54e7a 405
Lightvalve 169:645207e160ca 406 extern int cnt_freq_test;
Lightvalve 169:645207e160ca 407 extern int cnt_step_test;
Lightvalve 169:645207e160ca 408 extern int buffer_data_size;
Lightvalve 169:645207e160ca 409 extern int cnt_send_buffer;
Lightvalve 169:645207e160ca 410 extern float freq_test_valve_ref;
Lightvalve 169:645207e160ca 411 extern float ref_array[10000];
Lightvalve 169:645207e160ca 412 extern int pos_array[10000];
Lightvalve 192:637092202815 413 extern int flag_every_reference;
Lightvalve 169:645207e160ca 414
Lightvalve 14:8e7590227d22 415 extern int TMR3_COUNT_IREF;
Lightvalve 30:8d561f16383b 416 extern float CUR_CURRENT;
Lightvalve 30:8d561f16383b 417 extern float u_CUR[3];
Lightvalve 19:23b7c1ad8683 418
Lightvalve 29:69f3f5445d6d 419 extern int FINDHOME_STAGE;
Lightvalve 29:69f3f5445d6d 420 extern int FINDHOME_INIT;
Lightvalve 29:69f3f5445d6d 421 extern int FINDHOME_GOTOLIMIT;
Lightvalve 29:69f3f5445d6d 422 extern int FINDHOME_ZEROPOSE;
Lightvalve 29:69f3f5445d6d 423
Lightvalve 45:35fa6884d0c6 424 extern float alpha_trans;
Lightvalve 45:35fa6884d0c6 425
Lightvalve 57:f4819de54e7a 426
Lightvalve 57:f4819de54e7a 427 extern float V_out;
Lightvalve 57:f4819de54e7a 428 extern float V_rem;
Lightvalve 57:f4819de54e7a 429 extern float V_MAX;
Lightvalve 57:f4819de54e7a 430
Lightvalve 57:f4819de54e7a 431 extern float PWM_out;
Lightvalve 57:f4819de54e7a 432
Lightvalve 57:f4819de54e7a 433 extern double K_v;
Lightvalve 57:f4819de54e7a 434 extern double mV_PER_mA;
Lightvalve 57:f4819de54e7a 435 extern double mV_PER_pulse;
Lightvalve 57:f4819de54e7a 436 extern double mA_PER_pulse;
Lightvalve 57:f4819de54e7a 437
Lightvalve 57:f4819de54e7a 438 extern int timer_while;
Lightvalve 57:f4819de54e7a 439 extern int while_index;
Lightvalve 170:42c938a40313 440 extern int RL_timer;
Lightvalve 23:59218d4a256d 441
Lightvalve 67:c2812cf26c38 442 extern float K_LPF;
Lightvalve 67:c2812cf26c38 443 extern float D_LPF;
Lightvalve 67:c2812cf26c38 444
Lightvalve 207:c70c5a9f17dd 445 extern float totq_sen_past;
Lightvalve 207:c70c5a9f17dd 446 extern float torq_ref_past;
Lightvalve 99:7bbcb3c0fb06 447 extern float output_normalized;
Lightvalve 72:3436ce769b1e 448
Lightvalve 66:a8e6799dbce3 449
Lightvalve 230:eff425844edf 450 #define numpast_u 0
Lightvalve 97:d71c57e3515e 451 #define numpast_x 5
Lightvalve 122:dcb3ce3056a0 452 #define numfuture_x 0
Lightvalve 163:ba1290eb0341 453 #define numpast_f 0
Lightvalve 249:21430e06f706 454 #define numfuture_f 2
Lightvalve 249:21430e06f706 455 #define num_input 9 //numpast_x + 1 + numfuture_x + numpast_f + 1 + numfuture_f
Lightvalve 249:21430e06f706 456 //#define num_input 17 //numpast_x + 1 + numfuture_x + numpast_f + 1 + numfuture_f
Lightvalve 232:0287991a8615 457 //#define num_input 16 //numpast_x + 1 + numfuture_x + numpast_f + numfuture_f
Lightvalve 233:0a37cdd59651 458 //#define num_input 15
Lightvalve 85:a3b46118b5cd 459
Lightvalve 230:eff425844edf 460 #define num_array_u_past 1 // numpast_u * time_interval + 1
Lightvalve 249:21430e06f706 461 #define num_array_f_future 3 // numfuture_f * time_interval + 1
Lightvalve 163:ba1290eb0341 462 #define num_array_f_past 1 // numpast_f * time_interval + 1
Lightvalve 158:cb9a6999d5e2 463 #define num_array_x_past 6 // numpast_x * time_interval + 1
Lightvalve 122:dcb3ce3056a0 464 #define num_array_x_future 1 // numfuture_x * time_interval + 1
Lightvalve 118:af86e883dcb4 465
Lightvalve 158:cb9a6999d5e2 466 #define time_interval 1
Lightvalve 23:59218d4a256d 467
Lightvalve 23:59218d4a256d 468
Lightvalve 26:8317d883d4f2 469
Lightvalve 170:42c938a40313 470 /////////////RL
Lightvalve 170:42c938a40313 471 #define num_batch 10
Lightvalve 170:42c938a40313 472 #define num_epoch 1
Lightvalve 179:d5377766d7ea 473 #define batch_size 100
Lightvalve 170:42c938a40313 474 #define num_input_RL 2
Lightvalve 173:68c7914679ec 475 #define num_hidden_unit1 10
Lightvalve 173:68c7914679ec 476 #define num_hidden_unit2 10
Lightvalve 170:42c938a40313 477
Lightvalve 170:42c938a40313 478 extern int batch;
Lightvalve 170:42c938a40313 479 extern float train_set_x[batch_size];
Lightvalve 170:42c938a40313 480 extern float train_set_error[batch_size];
Lightvalve 170:42c938a40313 481 extern float train_set_count[batch_size];
Lightvalve 170:42c938a40313 482 extern float state_array[batch_size][num_input_RL];
Lightvalve 170:42c938a40313 483 extern float V[batch_size];
Lightvalve 170:42c938a40313 484 extern float r[batch_size];
Lightvalve 170:42c938a40313 485 extern float td_target[batch_size];
Lightvalve 170:42c938a40313 486 extern float delta[batch_size];
Lightvalve 170:42c938a40313 487 extern float advantage[batch_size];
Lightvalve 173:68c7914679ec 488 extern float return_G[batch_size];
Lightvalve 170:42c938a40313 489
Lightvalve 170:42c938a40313 490 extern float mean;
Lightvalve 170:42c938a40313 491 extern float deviation;
Lightvalve 170:42c938a40313 492 extern float mean_old;
Lightvalve 170:42c938a40313 493 extern float deviation_old;
Lightvalve 170:42c938a40313 494 extern float mean_before_SP;
Lightvalve 170:42c938a40313 495 extern float deviation_before_SP;
Lightvalve 170:42c938a40313 496 extern float mean_before_SP_array[batch_size];
Lightvalve 170:42c938a40313 497 extern float deviation_before_SP_array[batch_size];
Lightvalve 170:42c938a40313 498 extern float mean_array[batch_size];
Lightvalve 170:42c938a40313 499 extern float mean_array_old[batch_size];
Lightvalve 170:42c938a40313 500 extern float deviation_array[batch_size];
Lightvalve 170:42c938a40313 501 extern float deviation_array_old[batch_size];
Lightvalve 173:68c7914679ec 502
Lightvalve 173:68c7914679ec 503 extern float hx_c_sum[num_hidden_unit1];
Lightvalve 173:68c7914679ec 504 extern float hx_c_sum_array[batch_size][num_hidden_unit1];
Lightvalve 173:68c7914679ec 505 extern float hxh_c_sum[num_hidden_unit2];
Lightvalve 173:68c7914679ec 506 extern float hxh_c_sum_array[batch_size][num_hidden_unit2];
Lightvalve 173:68c7914679ec 507 extern float hxhh_c_sum;
Lightvalve 173:68c7914679ec 508 extern float hxhh_c_sum_array[batch_size];
Lightvalve 173:68c7914679ec 509
Lightvalve 173:68c7914679ec 510 extern float hx_a_sum[num_hidden_unit1];
Lightvalve 173:68c7914679ec 511 extern float hx_a_sum_array[batch_size][num_hidden_unit1];
Lightvalve 173:68c7914679ec 512 extern float hxh_a_sum[num_hidden_unit2];
Lightvalve 173:68c7914679ec 513 extern float hxh_a_sum_array[batch_size][num_hidden_unit2];
Lightvalve 173:68c7914679ec 514 extern float hxhh_a_sum[2];
Lightvalve 173:68c7914679ec 515 extern float hxhh_a_sum_array[batch_size][2];
Lightvalve 173:68c7914679ec 516
Lightvalve 170:42c938a40313 517 extern float action;
Lightvalve 170:42c938a40313 518 extern float action_array[batch_size];
Lightvalve 170:42c938a40313 519 extern float ratio[batch_size];
Lightvalve 170:42c938a40313 520 extern float pi[batch_size];
Lightvalve 170:42c938a40313 521 extern float pi_old[batch_size];
Lightvalve 170:42c938a40313 522 extern float epsilon;
Lightvalve 170:42c938a40313 523 extern float surr1[batch_size];
Lightvalve 170:42c938a40313 524 extern float surr2[batch_size];
Lightvalve 170:42c938a40313 525 extern float loss[batch_size];
Lightvalve 170:42c938a40313 526 extern float loss_batch;
Lightvalve 170:42c938a40313 527 extern float gamma;
Lightvalve 170:42c938a40313 528 extern float lmbda;
Lightvalve 170:42c938a40313 529 extern char Update_Done_Flag;
Lightvalve 170:42c938a40313 530 extern char Update_Case;
Lightvalve 170:42c938a40313 531 extern float reward_sum;
Lightvalve 170:42c938a40313 532
Lightvalve 170:42c938a40313 533 extern float virt_pos;
Lightvalve 170:42c938a40313 534 extern float logging1;
Lightvalve 170:42c938a40313 535 extern float logging2;
Lightvalve 170:42c938a40313 536 extern float logging3;
Lightvalve 170:42c938a40313 537 extern float logging4;
Lightvalve 170:42c938a40313 538 extern float logging5;
Lightvalve 30:8d561f16383b 539
Lightvalve 30:8d561f16383b 540
Lightvalve 32:4b8c0fedaf2c 541
Lightvalve 35:34ce7b0347b8 542
Lightvalve 36:a46e63505ed8 543
Lightvalve 45:35fa6884d0c6 544
Lightvalve 45:35fa6884d0c6 545
Lightvalve 48:889798ff9329 546
Lightvalve 49:e7bcfc244d40 547
Lightvalve 50:3c630b5eba9f 548
Lightvalve 52:8ea76864368a 549
Lightvalve 54:647072f5307a 550