![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
setting.h@171:bfc1fd2629d8, 2020-11-21 (annotated)
- Committer:
- Lightvalve
- Date:
- Sat Nov 21 07:15:47 2020 +0000
- Revision:
- 171:bfc1fd2629d8
- Parent:
- 170:42c938a40313
- Child:
- 173:68c7914679ec
201121-1
Who changed what in which revision?
User | Revision | Line number | New 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 | 170:42c938a40313 | 188 | extern float V_adapt; |
Lightvalve | 133:22ab22818e01 | 189 | extern float x_4_des_old; |
Lightvalve | 133:22ab22818e01 | 190 | |
Lightvalve | 57:f4819de54e7a | 191 | //extern int16_t VALVE_LIMIT_PLUS; |
Lightvalve | 57:f4819de54e7a | 192 | //extern int16_t VALVE_LIMIT_MINUS; |
Lightvalve | 11:82d8768d7351 | 193 | |
Lightvalve | 48:889798ff9329 | 194 | extern float ENC_PULSE_PER_POSITION; |
Lightvalve | 48:889798ff9329 | 195 | extern float TORQUE_SENSOR_PULSE_PER_TORQUE; |
Lightvalve | 48:889798ff9329 | 196 | |
Lightvalve | 30:8d561f16383b | 197 | extern float PRES_SENSOR_A_PULSE_PER_BAR; |
Lightvalve | 30:8d561f16383b | 198 | extern float PRES_SENSOR_B_PULSE_PER_BAR; |
Lightvalve | 11:82d8768d7351 | 199 | |
Lightvalve | 11:82d8768d7351 | 200 | extern int HOMEPOS_OFFSET; |
Lightvalve | 11:82d8768d7351 | 201 | extern int HOMEPOS_VALVE_OPENING; |
Lightvalve | 11:82d8768d7351 | 202 | |
Lightvalve | 30:8d561f16383b | 203 | extern float FRICTION; |
Lightvalve | 30:8d561f16383b | 204 | extern float REF_PERIOD; |
Lightvalve | 30:8d561f16383b | 205 | extern float REF_MAG; |
Lightvalve | 11:82d8768d7351 | 206 | extern int REF_NUM; |
Lightvalve | 11:82d8768d7351 | 207 | |
Lightvalve | 11:82d8768d7351 | 208 | |
Lightvalve | 30:8d561f16383b | 209 | extern float DAC_REF; |
Lightvalve | 30:8d561f16383b | 210 | extern float DAC_RESOL; |
Lightvalve | 11:82d8768d7351 | 211 | |
Lightvalve | 11:82d8768d7351 | 212 | extern int REF_POSITION; |
Lightvalve | 11:82d8768d7351 | 213 | extern int REF_VELOCITY; |
Lightvalve | 11:82d8768d7351 | 214 | extern int16_t REF_TORQUE; |
Lightvalve | 11:82d8768d7351 | 215 | extern int16_t REF_PRES_DIFF; |
Lightvalve | 11:82d8768d7351 | 216 | extern int16_t REF_PWM; |
Lightvalve | 11:82d8768d7351 | 217 | extern int16_t REF_VALVE_POSITION; |
Lightvalve | 14:8e7590227d22 | 218 | extern int16_t REF_CURRENT; |
Lightvalve | 11:82d8768d7351 | 219 | |
Lightvalve | 11:82d8768d7351 | 220 | extern int REF_MOVE_TIME_5k; |
Lightvalve | 11:82d8768d7351 | 221 | extern int INIT_REF_PWM; |
Lightvalve | 11:82d8768d7351 | 222 | extern int INIT_REF_VALVE_POS; |
Lightvalve | 11:82d8768d7351 | 223 | extern int INIT_REF_POS; |
Lightvalve | 11:82d8768d7351 | 224 | extern int INIT_REF_VEL; |
Lightvalve | 11:82d8768d7351 | 225 | extern int INIT_REF_TORQUE; |
Lightvalve | 11:82d8768d7351 | 226 | extern int INIT_REF_PRES_DIFF; |
Lightvalve | 14:8e7590227d22 | 227 | extern int INIT_REF_CURRENT; |
Lightvalve | 11:82d8768d7351 | 228 | |
Lightvalve | 11:82d8768d7351 | 229 | extern int CUR_POSITION; |
Lightvalve | 11:82d8768d7351 | 230 | extern int CUR_VELOCITY; |
Lightvalve | 30:8d561f16383b | 231 | extern float CUR_TORQUE; |
Lightvalve | 30:8d561f16383b | 232 | extern float CUR_PRES_A; |
Lightvalve | 30:8d561f16383b | 233 | extern float CUR_PRES_B; |
Lightvalve | 11:82d8768d7351 | 234 | extern int CUR_VALVE_POSITION; |
Lightvalve | 11:82d8768d7351 | 235 | |
Lightvalve | 11:82d8768d7351 | 236 | extern unsigned int TMR2_COUNT_LED1; |
Lightvalve | 11:82d8768d7351 | 237 | extern unsigned int TMR2_COUNT_LED2; |
Lightvalve | 11:82d8768d7351 | 238 | extern unsigned int TMR2_COUNT_CAN_TX; |
Lightvalve | 54:647072f5307a | 239 | extern unsigned int TMR3_COUNT_TEST; |
Lightvalve | 11:82d8768d7351 | 240 | |
Lightvalve | 11:82d8768d7351 | 241 | extern int num_err; |
Lightvalve | 11:82d8768d7351 | 242 | extern int flag_err[8]; |
Lightvalve | 11:82d8768d7351 | 243 | extern int flag_err_old[8]; |
Lightvalve | 11:82d8768d7351 | 244 | extern int flag_err_rt; |
Lightvalve | 11:82d8768d7351 | 245 | |
Lightvalve | 11:82d8768d7351 | 246 | extern int flag_ref_enable; |
Lightvalve | 11:82d8768d7351 | 247 | |
Lightvalve | 11:82d8768d7351 | 248 | extern int flag_data_request[5]; |
Lightvalve | 11:82d8768d7351 | 249 | |
Lightvalve | 45:35fa6884d0c6 | 250 | extern int MODE_POS_FT_TRANS; |
Lightvalve | 66:a8e6799dbce3 | 251 | extern int NN_Control_Flag; |
Lightvalve | 45:35fa6884d0c6 | 252 | |
Lightvalve | 169:645207e160ca | 253 | extern int cnt_buffer; |
Lightvalve | 169:645207e160ca | 254 | |
Lightvalve | 57:f4819de54e7a | 255 | extern float CUR_CURRENT_mA; |
Lightvalve | 30:8d561f16383b | 256 | extern float CUR_PRES_A_BAR; |
Lightvalve | 30:8d561f16383b | 257 | extern float CUR_PRES_B_BAR; |
Lightvalve | 30:8d561f16383b | 258 | extern float CUR_TORQUE_NM; |
Lightvalve | 30:8d561f16383b | 259 | extern float CUR_TORQUE_NM_PRESS; |
Lightvalve | 11:82d8768d7351 | 260 | |
Lightvalve | 30:8d561f16383b | 261 | extern float PRES_A_VREF; |
Lightvalve | 30:8d561f16383b | 262 | extern float PRES_B_VREF; |
Lightvalve | 30:8d561f16383b | 263 | extern float TORQUE_VREF; |
Lightvalve | 11:82d8768d7351 | 264 | |
Lightvalve | 30:8d561f16383b | 265 | extern float VALVE_PWM_RAW_FB; |
Lightvalve | 30:8d561f16383b | 266 | extern float VALVE_PWM_RAW_FF; |
Lightvalve | 57:f4819de54e7a | 267 | extern float VALVE_PWM_RAW; |
Lightvalve | 11:82d8768d7351 | 268 | extern int VALVE_PWM_VALVE_DZ; |
Lightvalve | 11:82d8768d7351 | 269 | |
Lightvalve | 30:8d561f16383b | 270 | extern float VALVE_GAIN_LPM_PER_V[10]; |
Lightvalve | 30:8d561f16383b | 271 | extern float VALVE_POS_VS_PWM[25]; |
Lightvalve | 11:82d8768d7351 | 272 | extern long JOINT_VEL[100]; |
Lightvalve | 11:82d8768d7351 | 273 | |
Lightvalve | 11:82d8768d7351 | 274 | extern int VALVE_MAX_POS; |
Lightvalve | 11:82d8768d7351 | 275 | extern int VALVE_MIN_POS; |
Lightvalve | 11:82d8768d7351 | 276 | extern int VALVE_POS_NUM; |
Lightvalve | 32:4b8c0fedaf2c | 277 | extern float VALVE_CENTER_OFFSET; |
Lightvalve | 33:91b17819ec30 | 278 | extern float VALVE_DZ_MINUS_OFFSET; |
Lightvalve | 33:91b17819ec30 | 279 | extern float VALVE_DZ_PLUS_OFFSET; |
Lightvalve | 33:91b17819ec30 | 280 | extern float VALVE_DEADZONE_MINUS; |
Lightvalve | 33:91b17819ec30 | 281 | extern float VALVE_DEADZONE_PLUS; |
Lightvalve | 11:82d8768d7351 | 282 | |
Lightvalve | 14:8e7590227d22 | 283 | extern int TMR3_COUNT_FINDHOME; |
Lightvalve | 14:8e7590227d22 | 284 | extern int TMR3_COUNT_FLOWRATE; |
Lightvalve | 14:8e7590227d22 | 285 | extern int TMR3_COUNT_DEADZONE; |
Lightvalve | 14:8e7590227d22 | 286 | extern int TMR3_COUNT_PRES_NULL; |
Lightvalve | 14:8e7590227d22 | 287 | extern int TMR3_COUNT_TORQUE_NULL; |
Lightvalve | 14:8e7590227d22 | 288 | extern int TMR3_COUNT_PRES_CALIB; |
Lightvalve | 14:8e7590227d22 | 289 | extern int TMR3_COUNT_REFERENCE; |
Lightvalve | 14:8e7590227d22 | 290 | extern int TMR3_COUNT_JOINT; |
Lightvalve | 14:8e7590227d22 | 291 | extern int TMR3_COUNT_ROTARY_FRIC_TUNE; |
Lightvalve | 11:82d8768d7351 | 292 | |
Lightvalve | 30:8d561f16383b | 293 | extern float TUNING_TIME; |
Lightvalve | 11:82d8768d7351 | 294 | |
Lightvalve | 30:8d561f16383b | 295 | extern float REFERENCE_FREQ; |
Lightvalve | 30:8d561f16383b | 296 | extern float REFERENCE_MAG; |
Lightvalve | 11:82d8768d7351 | 297 | |
Lightvalve | 11:82d8768d7351 | 298 | extern bool FLAG_FIND_HOME; |
Lightvalve | 11:82d8768d7351 | 299 | |
Lightvalve | 11:82d8768d7351 | 300 | extern int MODE_JUMP_STATUS; |
Lightvalve | 11:82d8768d7351 | 301 | |
Lightvalve | 30:8d561f16383b | 302 | extern float CUR_PRES_DIFF_BAR; |
Lightvalve | 30:8d561f16383b | 303 | extern float CUR_PRES_A_sum; |
Lightvalve | 30:8d561f16383b | 304 | extern float CUR_PRES_B_sum; |
Lightvalve | 30:8d561f16383b | 305 | extern float CUR_PRES_A_mean; |
Lightvalve | 30:8d561f16383b | 306 | extern float CUR_PRES_B_mean; |
Lightvalve | 30:8d561f16383b | 307 | extern float CUR_TORQUE_sum; |
Lightvalve | 30:8d561f16383b | 308 | extern float CUR_TORQUE_mean; |
Lightvalve | 30:8d561f16383b | 309 | extern float PRES_A_NULL; |
Lightvalve | 30:8d561f16383b | 310 | extern float PRES_B_NULL; |
Lightvalve | 30:8d561f16383b | 311 | extern float TORQUE_NULL; |
Lightvalve | 13:747daba9cf59 | 312 | |
Lightvalve | 30:8d561f16383b | 313 | extern float Ref_Valve_Pos_Old; |
Lightvalve | 13:747daba9cf59 | 314 | |
Lightvalve | 13:747daba9cf59 | 315 | extern int VALVE_ID_timer; |
Lightvalve | 13:747daba9cf59 | 316 | extern int VALVE_DZ_timer; |
Lightvalve | 13:747daba9cf59 | 317 | extern int VALVE_FR_timer; |
Lightvalve | 35:34ce7b0347b8 | 318 | extern int VALVE_HPL_timer; |
Lightvalve | 13:747daba9cf59 | 319 | extern int VALVE_POS_TMP; |
Lightvalve | 13:747daba9cf59 | 320 | extern int JOINT_VEL_TMP; |
Lightvalve | 13:747daba9cf59 | 321 | extern int DDV_POS_AVG ; |
Lightvalve | 19:23b7c1ad8683 | 322 | extern int VALVE_POS_AVG[50]; |
Lightvalve | 13:747daba9cf59 | 323 | extern int VALVE_POS_AVG_OLD; |
Lightvalve | 13:747daba9cf59 | 324 | extern int data_num; |
Lightvalve | 13:747daba9cf59 | 325 | extern int ID_index; |
Lightvalve | 13:747daba9cf59 | 326 | extern int DZ_index; |
Lightvalve | 19:23b7c1ad8683 | 327 | extern int ID_index_array[50]; |
Lightvalve | 13:747daba9cf59 | 328 | extern int first_check; |
Lightvalve | 30:8d561f16383b | 329 | extern float init_time; |
Lightvalve | 13:747daba9cf59 | 330 | extern int DZ_case; |
Lightvalve | 13:747daba9cf59 | 331 | extern int START_POS; |
Lightvalve | 13:747daba9cf59 | 332 | extern int FINAL_POS; |
Lightvalve | 13:747daba9cf59 | 333 | extern int DZ_DIRECTION; |
Lightvalve | 13:747daba9cf59 | 334 | extern int FIRST_DZ; |
Lightvalve | 13:747daba9cf59 | 335 | extern int SECOND_DZ; |
Lightvalve | 13:747daba9cf59 | 336 | extern int DZ_NUM; |
Lightvalve | 13:747daba9cf59 | 337 | extern int one_period_end; |
Lightvalve | 30:8d561f16383b | 338 | extern float Ref_Vel_Test; |
Lightvalve | 13:747daba9cf59 | 339 | extern long TMR2_FOR_SLOW_LOGGING; |
Lightvalve | 19:23b7c1ad8683 | 340 | //extern int velcount; |
Lightvalve | 13:747daba9cf59 | 341 | extern char max_check; |
Lightvalve | 13:747daba9cf59 | 342 | extern char min_check; |
Lightvalve | 13:747daba9cf59 | 343 | |
Lightvalve | 30:8d561f16383b | 344 | extern float valve_pos_err, valve_pos_err_old, valve_pos_err_diff, valve_pos_err_sum; |
Lightvalve | 30:8d561f16383b | 345 | 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 | 346 | extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum; |
Lightvalve | 30:8d561f16383b | 347 | extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ; |
Lightvalve | 13:747daba9cf59 | 348 | |
Lightvalve | 30:8d561f16383b | 349 | extern float CUR_FLOWRATE; |
Lightvalve | 30:8d561f16383b | 350 | extern float VALVE_FF_VOLTAGE; |
Lightvalve | 13:747daba9cf59 | 351 | |
Lightvalve | 13:747daba9cf59 | 352 | extern int pos_plus_end; |
Lightvalve | 13:747daba9cf59 | 353 | extern int pos_minus_end; |
Lightvalve | 13:747daba9cf59 | 354 | |
Lightvalve | 13:747daba9cf59 | 355 | extern bool need_enc_init; |
Lightvalve | 13:747daba9cf59 | 356 | |
Lightvalve | 13:747daba9cf59 | 357 | extern int temp_time; |
Lightvalve | 13:747daba9cf59 | 358 | |
Lightvalve | 30:8d561f16383b | 359 | extern float CUR_VELOCITY_sum; |
Lightvalve | 30:8d561f16383b | 360 | extern float temp_vel_sum; |
Lightvalve | 13:747daba9cf59 | 361 | |
Lightvalve | 13:747daba9cf59 | 362 | extern int DZ_dir; |
Lightvalve | 13:747daba9cf59 | 363 | extern int DZ_temp_cnt; |
Lightvalve | 13:747daba9cf59 | 364 | extern int DZ_temp_cnt2; |
Lightvalve | 13:747daba9cf59 | 365 | extern int DZ_end; |
Lightvalve | 13:747daba9cf59 | 366 | extern int flag_flowrate; |
Lightvalve | 13:747daba9cf59 | 367 | extern int fl_temp_cnt; |
Lightvalve | 13:747daba9cf59 | 368 | extern int fl_temp_cnt2; |
Lightvalve | 13:747daba9cf59 | 369 | extern int cur_vel_sum; |
Lightvalve | 13:747daba9cf59 | 370 | |
Lightvalve | 57:f4819de54e7a | 371 | extern float Cur_Valve_Open_pulse; |
Lightvalve | 57:f4819de54e7a | 372 | |
Lightvalve | 13:747daba9cf59 | 373 | // find home |
Lightvalve | 13:747daba9cf59 | 374 | extern int CUR_VELOCITY_OLD; |
Lightvalve | 13:747daba9cf59 | 375 | extern int cnt_findhome; |
Lightvalve | 13:747daba9cf59 | 376 | extern int cnt_vel_findhome; |
Lightvalve | 13:747daba9cf59 | 377 | extern int FINDHOME_VELOCITY; |
Lightvalve | 13:747daba9cf59 | 378 | extern int FINDHOME_VELOCITY_OLD; |
Lightvalve | 13:747daba9cf59 | 379 | extern int FINDHOME_POSITION; |
Lightvalve | 13:747daba9cf59 | 380 | extern int FINDHOME_POSITION_OLD; |
Lightvalve | 13:747daba9cf59 | 381 | |
Lightvalve | 13:747daba9cf59 | 382 | // valve gain |
Lightvalve | 13:747daba9cf59 | 383 | extern int check_vel_pos_init; |
Lightvalve | 13:747daba9cf59 | 384 | extern int check_vel_pos_fin; |
Lightvalve | 13:747daba9cf59 | 385 | extern int check_vel_pos_interv; |
Lightvalve | 13:747daba9cf59 | 386 | extern int valve_gain_repeat_cnt; |
Lightvalve | 30:8d561f16383b | 387 | extern float VALVE_VOLTAGE; |
Lightvalve | 13:747daba9cf59 | 388 | |
Lightvalve | 30:8d561f16383b | 389 | extern float freq_fric_tune; |
Lightvalve | 13:747daba9cf59 | 390 | |
Lightvalve | 14:8e7590227d22 | 391 | extern uint32_t TMR3_COUNT_CAN_TX; |
Lightvalve | 14:8e7590227d22 | 392 | |
Lightvalve | 57:f4819de54e7a | 393 | // Current Control Variables |
Lightvalve | 57:f4819de54e7a | 394 | extern double I_REF; |
Lightvalve | 57:f4819de54e7a | 395 | extern double I_REF_fil; |
Lightvalve | 57:f4819de54e7a | 396 | extern double I_ERR; |
Lightvalve | 57:f4819de54e7a | 397 | extern double I_ERR_INT; |
Lightvalve | 57:f4819de54e7a | 398 | extern double I_REF_fil_old; |
Lightvalve | 57:f4819de54e7a | 399 | extern double I_REF_fil_diff; |
Lightvalve | 57:f4819de54e7a | 400 | |
Lightvalve | 57:f4819de54e7a | 401 | // system id |
Lightvalve | 57:f4819de54e7a | 402 | extern int cnt_sysid; |
Lightvalve | 57:f4819de54e7a | 403 | extern double freq_sysid_Iref; |
Lightvalve | 57:f4819de54e7a | 404 | |
Lightvalve | 169:645207e160ca | 405 | extern int cnt_freq_test; |
Lightvalve | 169:645207e160ca | 406 | extern int cnt_step_test; |
Lightvalve | 169:645207e160ca | 407 | extern int buffer_data_size; |
Lightvalve | 169:645207e160ca | 408 | extern int cnt_send_buffer; |
Lightvalve | 169:645207e160ca | 409 | extern float freq_test_valve_ref; |
Lightvalve | 169:645207e160ca | 410 | extern float ref_array[10000]; |
Lightvalve | 169:645207e160ca | 411 | extern int pos_array[10000]; |
Lightvalve | 169:645207e160ca | 412 | |
Lightvalve | 14:8e7590227d22 | 413 | extern int TMR3_COUNT_IREF; |
Lightvalve | 30:8d561f16383b | 414 | extern float CUR_CURRENT; |
Lightvalve | 30:8d561f16383b | 415 | extern float u_CUR[3]; |
Lightvalve | 19:23b7c1ad8683 | 416 | |
Lightvalve | 29:69f3f5445d6d | 417 | extern int FINDHOME_STAGE; |
Lightvalve | 29:69f3f5445d6d | 418 | extern int FINDHOME_INIT; |
Lightvalve | 29:69f3f5445d6d | 419 | extern int FINDHOME_GOTOLIMIT; |
Lightvalve | 29:69f3f5445d6d | 420 | extern int FINDHOME_ZEROPOSE; |
Lightvalve | 29:69f3f5445d6d | 421 | |
Lightvalve | 45:35fa6884d0c6 | 422 | extern float alpha_trans; |
Lightvalve | 45:35fa6884d0c6 | 423 | |
Lightvalve | 57:f4819de54e7a | 424 | |
Lightvalve | 57:f4819de54e7a | 425 | extern float V_out; |
Lightvalve | 57:f4819de54e7a | 426 | extern float V_rem; |
Lightvalve | 57:f4819de54e7a | 427 | extern float V_MAX; |
Lightvalve | 57:f4819de54e7a | 428 | |
Lightvalve | 57:f4819de54e7a | 429 | extern float PWM_out; |
Lightvalve | 57:f4819de54e7a | 430 | |
Lightvalve | 57:f4819de54e7a | 431 | extern double K_v; |
Lightvalve | 57:f4819de54e7a | 432 | extern double mV_PER_mA; |
Lightvalve | 57:f4819de54e7a | 433 | extern double mV_PER_pulse; |
Lightvalve | 57:f4819de54e7a | 434 | extern double mA_PER_pulse; |
Lightvalve | 57:f4819de54e7a | 435 | |
Lightvalve | 57:f4819de54e7a | 436 | extern int timer_while; |
Lightvalve | 57:f4819de54e7a | 437 | extern int while_index; |
Lightvalve | 170:42c938a40313 | 438 | extern int RL_timer; |
Lightvalve | 23:59218d4a256d | 439 | |
Lightvalve | 67:c2812cf26c38 | 440 | extern float K_LPF; |
Lightvalve | 67:c2812cf26c38 | 441 | extern float D_LPF; |
Lightvalve | 67:c2812cf26c38 | 442 | |
Lightvalve | 72:3436ce769b1e | 443 | extern float torq_ref_past ; |
Lightvalve | 99:7bbcb3c0fb06 | 444 | extern float output_normalized; |
Lightvalve | 72:3436ce769b1e | 445 | |
Lightvalve | 66:a8e6799dbce3 | 446 | //extern const int num_input ; |
Lightvalve | 66:a8e6799dbce3 | 447 | //extern const int num_input_array; |
Lightvalve | 66:a8e6799dbce3 | 448 | |
Lightvalve | 127:15751f2bad9b | 449 | #define numpast_u 0 |
Lightvalve | 97:d71c57e3515e | 450 | #define numpast_x 5 |
Lightvalve | 122:dcb3ce3056a0 | 451 | #define numfuture_x 0 |
Lightvalve | 163:ba1290eb0341 | 452 | #define numpast_f 0 |
Lightvalve | 81:49a733a3eb9e | 453 | #define numfuture_f 10 |
Lightvalve | 163:ba1290eb0341 | 454 | #define num_input 17 //numpast_x + 1 + numfuture_x + numpast_f + 1 + numfuture_f |
Lightvalve | 85:a3b46118b5cd | 455 | |
Lightvalve | 127:15751f2bad9b | 456 | #define num_array_u_past 1 // numpast_u * time_interval + 1 |
Lightvalve | 158:cb9a6999d5e2 | 457 | #define num_array_f_future 11 // numfuture_f * time_interval + 1 |
Lightvalve | 163:ba1290eb0341 | 458 | #define num_array_f_past 1 // numpast_f * time_interval + 1 |
Lightvalve | 158:cb9a6999d5e2 | 459 | #define num_array_x_past 6 // numpast_x * time_interval + 1 |
Lightvalve | 122:dcb3ce3056a0 | 460 | #define num_array_x_future 1 // numfuture_x * time_interval + 1 |
Lightvalve | 118:af86e883dcb4 | 461 | |
Lightvalve | 158:cb9a6999d5e2 | 462 | #define time_interval 1 |
Lightvalve | 23:59218d4a256d | 463 | |
Lightvalve | 23:59218d4a256d | 464 | |
Lightvalve | 26:8317d883d4f2 | 465 | |
Lightvalve | 170:42c938a40313 | 466 | /////////////RL |
Lightvalve | 170:42c938a40313 | 467 | #define num_batch 10 |
Lightvalve | 170:42c938a40313 | 468 | #define num_epoch 1 |
Lightvalve | 170:42c938a40313 | 469 | #define batch_size 50 |
Lightvalve | 170:42c938a40313 | 470 | #define num_input_RL 2 |
Lightvalve | 171:bfc1fd2629d8 | 471 | #define num_hidden_unit 10 |
Lightvalve | 170:42c938a40313 | 472 | |
Lightvalve | 170:42c938a40313 | 473 | extern int batch; |
Lightvalve | 170:42c938a40313 | 474 | extern float train_set_x[batch_size]; |
Lightvalve | 170:42c938a40313 | 475 | extern float train_set_error[batch_size]; |
Lightvalve | 170:42c938a40313 | 476 | extern float train_set_count[batch_size]; |
Lightvalve | 170:42c938a40313 | 477 | extern float state_array[batch_size][num_input_RL]; |
Lightvalve | 170:42c938a40313 | 478 | extern float V[batch_size]; |
Lightvalve | 170:42c938a40313 | 479 | extern float r[batch_size]; |
Lightvalve | 170:42c938a40313 | 480 | extern float td_target[batch_size]; |
Lightvalve | 170:42c938a40313 | 481 | extern float delta[batch_size]; |
Lightvalve | 170:42c938a40313 | 482 | extern float advantage[batch_size]; |
Lightvalve | 170:42c938a40313 | 483 | |
Lightvalve | 170:42c938a40313 | 484 | extern float mean; |
Lightvalve | 170:42c938a40313 | 485 | extern float deviation; |
Lightvalve | 170:42c938a40313 | 486 | extern float mean_old; |
Lightvalve | 170:42c938a40313 | 487 | extern float deviation_old; |
Lightvalve | 170:42c938a40313 | 488 | extern float mean_before_SP; |
Lightvalve | 170:42c938a40313 | 489 | extern float deviation_before_SP; |
Lightvalve | 170:42c938a40313 | 490 | extern float mean_before_SP_array[batch_size]; |
Lightvalve | 170:42c938a40313 | 491 | extern float deviation_before_SP_array[batch_size]; |
Lightvalve | 170:42c938a40313 | 492 | extern float mean_array[batch_size]; |
Lightvalve | 170:42c938a40313 | 493 | extern float mean_array_old[batch_size]; |
Lightvalve | 170:42c938a40313 | 494 | extern float deviation_array[batch_size]; |
Lightvalve | 170:42c938a40313 | 495 | extern float deviation_array_old[batch_size]; |
Lightvalve | 170:42c938a40313 | 496 | extern float action; |
Lightvalve | 170:42c938a40313 | 497 | extern float action_array[batch_size]; |
Lightvalve | 170:42c938a40313 | 498 | extern float ratio[batch_size]; |
Lightvalve | 170:42c938a40313 | 499 | extern float pi[batch_size]; |
Lightvalve | 170:42c938a40313 | 500 | extern float pi_old[batch_size]; |
Lightvalve | 170:42c938a40313 | 501 | extern float epsilon; |
Lightvalve | 170:42c938a40313 | 502 | extern float surr1[batch_size]; |
Lightvalve | 170:42c938a40313 | 503 | extern float surr2[batch_size]; |
Lightvalve | 170:42c938a40313 | 504 | extern float loss[batch_size]; |
Lightvalve | 170:42c938a40313 | 505 | extern float loss_batch; |
Lightvalve | 170:42c938a40313 | 506 | extern float gamma; |
Lightvalve | 170:42c938a40313 | 507 | extern float lmbda; |
Lightvalve | 170:42c938a40313 | 508 | extern char Update_Done_Flag; |
Lightvalve | 170:42c938a40313 | 509 | extern char Update_Case; |
Lightvalve | 170:42c938a40313 | 510 | extern float reward_sum; |
Lightvalve | 170:42c938a40313 | 511 | |
Lightvalve | 170:42c938a40313 | 512 | extern float virt_pos; |
Lightvalve | 170:42c938a40313 | 513 | extern float logging1; |
Lightvalve | 170:42c938a40313 | 514 | extern float logging2; |
Lightvalve | 170:42c938a40313 | 515 | extern float logging3; |
Lightvalve | 170:42c938a40313 | 516 | extern float logging4; |
Lightvalve | 170:42c938a40313 | 517 | extern float logging5; |
Lightvalve | 30:8d561f16383b | 518 | |
Lightvalve | 30:8d561f16383b | 519 | |
Lightvalve | 32:4b8c0fedaf2c | 520 | |
Lightvalve | 35:34ce7b0347b8 | 521 | |
Lightvalve | 36:a46e63505ed8 | 522 | |
Lightvalve | 45:35fa6884d0c6 | 523 | |
Lightvalve | 45:35fa6884d0c6 | 524 | |
Lightvalve | 48:889798ff9329 | 525 | |
Lightvalve | 49:e7bcfc244d40 | 526 | |
Lightvalve | 50:3c630b5eba9f | 527 | |
Lightvalve | 52:8ea76864368a | 528 | |
Lightvalve | 54:647072f5307a | 529 |