2011

Dependencies:   mbed FastPWM

Committer:
Lightvalve
Date:
Fri Aug 23 12:28:08 2019 +0000
Revision:
11:82d8768d7351
Parent:
7:e9086c72bb22
Child:
13:747daba9cf59
190823

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
jobuuu 2:a1c0a37df760 9 #define PWM_ARR 0x1194 // loop 20k, pwm 10k
jobuuu 2:a1c0a37df760 10 //#define PWM_ARR 0x2328 // loop 10k, pwm 5k
Lightvalve 11:82d8768d7351 11 #define TMR3_COUNT 0x4650 // loop 5k
jobuuu 2:a1c0a37df760 12
jobuuu 2:a1c0a37df760 13 #define FREQ_5k 5000.0
jobuuu 2:a1c0a37df760 14 #define FREQ_10k 10000.0
jobuuu 2:a1c0a37df760 15 #define FREQ_20k 20000.0
jobuuu 2:a1c0a37df760 16 #define FREQ_40k 40000.0
jobuuu 5:a4319f79457b 17 #define DT_5k 0.0002
jobuuu 5:a4319f79457b 18 #define DT_10k 0.0001
jobuuu 5:a4319f79457b 19 #define DT_20k 0.00005
jobuuu 5:a4319f79457b 20 #define DT_40k 0.000025
jobuuu 5:a4319f79457b 21
jobuuu 7:e9086c72bb22 22 extern DigitalOut check;
jobuuu 7:e9086c72bb22 23 extern DigitalOut check_2;
jobuuu 7:e9086c72bb22 24 extern AnalogOut dac_1;
jobuuu 7:e9086c72bb22 25 extern AnalogOut dac_2;
jobuuu 7:e9086c72bb22 26
jobuuu 2:a1c0a37df760 27 extern double dtc_v;
jobuuu 2:a1c0a37df760 28 extern double dtc_w;
GiJeongKim 0:51c43836c1d7 29
GiJeongKim 0:51c43836c1d7 30 // I2C
GiJeongKim 0:51c43836c1d7 31 extern I2C i2c; // SDA, SCL (for K22F)
GiJeongKim 0:51c43836c1d7 32 extern const int i2c_slave_addr1;
GiJeongKim 0:51c43836c1d7 33 extern unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim 0:51c43836c1d7 34
GiJeongKim 0:51c43836c1d7 35 // SPI
GiJeongKim 0:51c43836c1d7 36 extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK);
GiJeongKim 0:51c43836c1d7 37 extern DigitalOut eeprom_cs;
GiJeongKim 0:51c43836c1d7 38 extern SPI enc;
GiJeongKim 0:51c43836c1d7 39 extern DigitalOut enc_cs;
GiJeongKim 0:51c43836c1d7 40
GiJeongKim 0:51c43836c1d7 41 // UART
jobuuu 7:e9086c72bb22 42 extern Serial pc; //Serial pc(PA_9,PA_10); _ UART
jobuuu 7:e9086c72bb22 43
jobuuu 7:e9086c72bb22 44 // CAN
jobuuu 7:e9086c72bb22 45 extern CAN can;
jobuuu 7:e9086c72bb22 46 extern CANMessage msg;
jobuuu 7:e9086c72bb22 47
jobuuu 7:e9086c72bb22 48 // Board Information
jobuuu 7:e9086c72bb22 49
jobuuu 7:e9086c72bb22 50
jobuuu 7:e9086c72bb22 51 // User Function
jobuuu 7:e9086c72bb22 52
jobuuu 7:e9086c72bb22 53 void CurrentControl();
jobuuu 7:e9086c72bb22 54 void ValveControl(unsigned int ControlMode);
Lightvalve 11:82d8768d7351 55
Lightvalve 11:82d8768d7351 56
Lightvalve 11:82d8768d7351 57 /*******************************************************************************
Lightvalve 11:82d8768d7351 58 * COMMON Settings
Lightvalve 11:82d8768d7351 59 ******************************************************************************/
Lightvalve 11:82d8768d7351 60 #define LATEST_VERSION 19032
Lightvalve 11:82d8768d7351 61
Lightvalve 11:82d8768d7351 62
Lightvalve 11:82d8768d7351 63
Lightvalve 11:82d8768d7351 64 /*******************************************************************************
Lightvalve 11:82d8768d7351 65 * COMMON CONSTANTS
Lightvalve 11:82d8768d7351 66 ******************************************************************************/
Lightvalve 11:82d8768d7351 67 #define RAD_30 0.523598775598299
Lightvalve 11:82d8768d7351 68 #define RAD_60 1.047197551196598
Lightvalve 11:82d8768d7351 69 #define RAD_120 2.094395102393195
Lightvalve 11:82d8768d7351 70 #define RAD_180 3.141592653589793
Lightvalve 11:82d8768d7351 71 #define RAD_240 4.188790204786391
Lightvalve 11:82d8768d7351 72 #define RAD_300 5.235987755982989
Lightvalve 11:82d8768d7351 73 #define RAD_360 6.283185307179586
Lightvalve 11:82d8768d7351 74
Lightvalve 11:82d8768d7351 75 #define SYSFREQ 200000000
Lightvalve 11:82d8768d7351 76 #define PBCLK 100000000
Lightvalve 11:82d8768d7351 77
Lightvalve 11:82d8768d7351 78 //#define TMR_FREQ_10k 10000
Lightvalve 11:82d8768d7351 79 #define TMR_FREQ_5k 5000
Lightvalve 11:82d8768d7351 80
Lightvalve 11:82d8768d7351 81 #define FALSE 0
Lightvalve 11:82d8768d7351 82 #define TRUE 1
Lightvalve 11:82d8768d7351 83 #define OUTPUT 0
Lightvalve 11:82d8768d7351 84 #define INPUT 1
Lightvalve 11:82d8768d7351 85 #define LOW 0
Lightvalve 11:82d8768d7351 86 #define HIGH 1
Lightvalve 11:82d8768d7351 87 #define DIGITAL 0
Lightvalve 11:82d8768d7351 88 #define ANALOG 1
Lightvalve 11:82d8768d7351 89
Lightvalve 11:82d8768d7351 90 #define PI 3.141592653589793
Lightvalve 11:82d8768d7351 91 #define D2R 0.017453292519943
Lightvalve 11:82d8768d7351 92 #define R2D 57.295779513082323
Lightvalve 11:82d8768d7351 93
Lightvalve 11:82d8768d7351 94
Lightvalve 11:82d8768d7351 95 /*******************************************************************************
Lightvalve 11:82d8768d7351 96 * HEADER INCLUDE
Lightvalve 11:82d8768d7351 97 ******************************************************************************/
Lightvalve 11:82d8768d7351 98
Lightvalve 11:82d8768d7351 99 /*******************************************************************************
Lightvalve 11:82d8768d7351 100 * VARIABLE
Lightvalve 11:82d8768d7351 101 ******************************************************************************/
Lightvalve 11:82d8768d7351 102
Lightvalve 11:82d8768d7351 103 // Board Information
Lightvalve 11:82d8768d7351 104 extern uint8_t BNO;
Lightvalve 11:82d8768d7351 105 extern uint8_t CONTROL_MODE;
Lightvalve 11:82d8768d7351 106 extern uint8_t OPERATING_MODE;
Lightvalve 11:82d8768d7351 107 extern uint8_t SETTING_SWITCH;
Lightvalve 11:82d8768d7351 108 extern uint8_t SETTING_SWITCH_OLD;
Lightvalve 11:82d8768d7351 109 extern uint8_t REFERENCE_MODE;
Lightvalve 11:82d8768d7351 110 extern uint16_t CAN_FREQ;
Lightvalve 11:82d8768d7351 111 extern uint8_t DIR_JOINT_ENC;
Lightvalve 11:82d8768d7351 112 extern uint8_t DIR_VALVE;
Lightvalve 11:82d8768d7351 113 extern uint8_t DIR_VALVE_ENC;
Lightvalve 11:82d8768d7351 114
Lightvalve 11:82d8768d7351 115 extern double SUPPLY_VOLTAGE;
Lightvalve 11:82d8768d7351 116 extern double VALVE_VOLTAGE_LIMIT;
Lightvalve 11:82d8768d7351 117
Lightvalve 11:82d8768d7351 118 extern double P_GAIN_VALVE_POSITION;
Lightvalve 11:82d8768d7351 119 extern double I_GAIN_VALVE_POSITION;
Lightvalve 11:82d8768d7351 120 extern double D_GAIN_VALVE_POSITION;
Lightvalve 11:82d8768d7351 121 extern double P_GAIN_JOINT_POSITION;
Lightvalve 11:82d8768d7351 122 extern double I_GAIN_JOINT_POSITION;
Lightvalve 11:82d8768d7351 123 extern double D_GAIN_JOINT_POSITION;
Lightvalve 11:82d8768d7351 124 extern double P_GAIN_JOINT_TORQUE;
Lightvalve 11:82d8768d7351 125 extern double I_GAIN_JOINT_TORQUE;
Lightvalve 11:82d8768d7351 126 extern double D_GAIN_JOINT_TORQUE;
Lightvalve 11:82d8768d7351 127
Lightvalve 11:82d8768d7351 128 extern int16_t VALVE_DEADZONE_PLUS;
Lightvalve 11:82d8768d7351 129 extern int16_t VALVE_DEADZONE_MINUS;
Lightvalve 11:82d8768d7351 130
Lightvalve 11:82d8768d7351 131 extern int16_t VELOCITY_COMP_GAIN;
Lightvalve 11:82d8768d7351 132 extern int16_t COMPLIANCE_GAIN;
Lightvalve 11:82d8768d7351 133
Lightvalve 11:82d8768d7351 134 extern int16_t VALVE_CENTER;
Lightvalve 11:82d8768d7351 135
Lightvalve 11:82d8768d7351 136 extern int16_t VALVE_FF;
Lightvalve 11:82d8768d7351 137
Lightvalve 11:82d8768d7351 138 extern int16_t BULK_MODULUS;
Lightvalve 11:82d8768d7351 139
Lightvalve 11:82d8768d7351 140 extern int16_t CHAMBER_VOLUME_A;
Lightvalve 11:82d8768d7351 141 extern int16_t CHAMBER_VOLUME_B;
Lightvalve 11:82d8768d7351 142
Lightvalve 11:82d8768d7351 143 extern int16_t PISTON_AREA_A;
Lightvalve 11:82d8768d7351 144 extern int16_t PISTON_AREA_B;
Lightvalve 11:82d8768d7351 145 extern double PISTON_AREA_alpha;
Lightvalve 11:82d8768d7351 146
Lightvalve 11:82d8768d7351 147
Lightvalve 11:82d8768d7351 148 extern int16_t PRES_SUPPLY;
Lightvalve 11:82d8768d7351 149 extern int16_t PRES_RETURN;
Lightvalve 11:82d8768d7351 150
Lightvalve 11:82d8768d7351 151 extern int16_t ENC_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 152 extern int16_t ENC_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 153
Lightvalve 11:82d8768d7351 154 extern int16_t STROKE;
Lightvalve 11:82d8768d7351 155
Lightvalve 11:82d8768d7351 156
Lightvalve 11:82d8768d7351 157 extern int16_t VALVE_LIMIT_PLUS;
Lightvalve 11:82d8768d7351 158 extern int16_t VALVE_LIMIT_MINUS;
Lightvalve 11:82d8768d7351 159
Lightvalve 11:82d8768d7351 160 extern int16_t ENC_PULSE_PER_POSITION;
Lightvalve 11:82d8768d7351 161 extern int16_t TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 11:82d8768d7351 162 extern double PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve 11:82d8768d7351 163 extern double PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve 11:82d8768d7351 164
Lightvalve 11:82d8768d7351 165 extern int HOMEPOS_OFFSET;
Lightvalve 11:82d8768d7351 166 extern int HOMEPOS_VALVE_OPENING;
Lightvalve 11:82d8768d7351 167
Lightvalve 11:82d8768d7351 168 extern double FRICTION;
Lightvalve 11:82d8768d7351 169 extern double REF_PERIOD;
Lightvalve 11:82d8768d7351 170 extern double REF_MAG;
Lightvalve 11:82d8768d7351 171 extern int REF_NUM;
Lightvalve 11:82d8768d7351 172
Lightvalve 11:82d8768d7351 173
Lightvalve 11:82d8768d7351 174 extern double DAC_REF;
Lightvalve 11:82d8768d7351 175 extern double DAC_RESOL;
Lightvalve 11:82d8768d7351 176 extern double PWM_RESOL;
Lightvalve 11:82d8768d7351 177
Lightvalve 11:82d8768d7351 178 extern int REF_POSITION;
Lightvalve 11:82d8768d7351 179 extern int REF_VELOCITY;
Lightvalve 11:82d8768d7351 180 extern int16_t REF_TORQUE;
Lightvalve 11:82d8768d7351 181 extern int16_t REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 182 extern int16_t REF_PWM;
Lightvalve 11:82d8768d7351 183 extern int16_t REF_VALVE_POSITION;
Lightvalve 11:82d8768d7351 184
Lightvalve 11:82d8768d7351 185 extern int REF_MOVE_TIME_5k;
Lightvalve 11:82d8768d7351 186 extern int INIT_REF_PWM;
Lightvalve 11:82d8768d7351 187 extern int INIT_REF_VALVE_POS;
Lightvalve 11:82d8768d7351 188 extern int INIT_REF_POS;
Lightvalve 11:82d8768d7351 189 extern int INIT_REF_VEL;
Lightvalve 11:82d8768d7351 190 extern int INIT_REF_TORQUE;
Lightvalve 11:82d8768d7351 191 extern int INIT_REF_PRES_DIFF;
Lightvalve 11:82d8768d7351 192
Lightvalve 11:82d8768d7351 193 extern int CUR_POSITION;
Lightvalve 11:82d8768d7351 194 extern int CUR_VELOCITY;
Lightvalve 11:82d8768d7351 195 extern double CUR_TORQUE;
Lightvalve 11:82d8768d7351 196 extern double CUR_PRES_A;
Lightvalve 11:82d8768d7351 197 extern double CUR_PRES_B;
Lightvalve 11:82d8768d7351 198 extern int CUR_PWM;
Lightvalve 11:82d8768d7351 199 extern int CUR_VALVE_POSITION;
Lightvalve 11:82d8768d7351 200
Lightvalve 11:82d8768d7351 201 extern unsigned int TMR2_COUNT_LED1;
Lightvalve 11:82d8768d7351 202 extern unsigned int TMR2_COUNT_LED2;
Lightvalve 11:82d8768d7351 203 extern unsigned int TMR2_COUNT_CAN_TX;
Lightvalve 11:82d8768d7351 204
Lightvalve 11:82d8768d7351 205 extern int num_err;
Lightvalve 11:82d8768d7351 206 extern int flag_err[8];
Lightvalve 11:82d8768d7351 207 extern int flag_err_old[8];
Lightvalve 11:82d8768d7351 208 extern int flag_err_rt;
Lightvalve 11:82d8768d7351 209
Lightvalve 11:82d8768d7351 210 extern int flag_ref_enable;
Lightvalve 11:82d8768d7351 211
Lightvalve 11:82d8768d7351 212 extern int flag_data_request[5];
Lightvalve 11:82d8768d7351 213
Lightvalve 11:82d8768d7351 214 ////////////////////////////////////////////////////////////////////////////////
Lightvalve 11:82d8768d7351 215 ////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
Lightvalve 11:82d8768d7351 216 ////////////////////////////////////////////////////////////////////////////////
Lightvalve 11:82d8768d7351 217
Lightvalve 11:82d8768d7351 218 extern double CUR_PRES_A_BAR;
Lightvalve 11:82d8768d7351 219 extern double CUR_PRES_B_BAR;
Lightvalve 11:82d8768d7351 220 extern double CUR_TORQUE_NM;
Lightvalve 11:82d8768d7351 221 extern double CUR_TORQUE_NM_PRESS;
Lightvalve 11:82d8768d7351 222
Lightvalve 11:82d8768d7351 223 extern double PRES_A_VREF;
Lightvalve 11:82d8768d7351 224 extern double PRES_B_VREF;
Lightvalve 11:82d8768d7351 225 extern double TORQUE_VREF;
Lightvalve 11:82d8768d7351 226
Lightvalve 11:82d8768d7351 227 extern double VALVE_PWM_RAW;
Lightvalve 11:82d8768d7351 228 extern double VALVE_PWM_RAW_FB;
Lightvalve 11:82d8768d7351 229 extern double VALVE_PWM_RAW_FF;
Lightvalve 11:82d8768d7351 230 extern int VALVE_PWM_VALVE_DZ;
Lightvalve 11:82d8768d7351 231 extern int VALVE_INPUT_PWM;
Lightvalve 11:82d8768d7351 232
Lightvalve 11:82d8768d7351 233 extern double VALVE_GAIN_LPM_PER_V[10];
Lightvalve 11:82d8768d7351 234 extern double VALVE_POS_VS_PWM[18];
Lightvalve 11:82d8768d7351 235 extern long JOINT_VEL[100];
Lightvalve 11:82d8768d7351 236
Lightvalve 11:82d8768d7351 237 extern int VALVE_MAX_POS;
Lightvalve 11:82d8768d7351 238 extern int VALVE_MIN_POS;
Lightvalve 11:82d8768d7351 239 extern int DDV_CENTER;
Lightvalve 11:82d8768d7351 240 extern int VALVE_POS_NUM;
Lightvalve 11:82d8768d7351 241
Lightvalve 11:82d8768d7351 242 extern int TMR2_COUNT_FINDHOME;
Lightvalve 11:82d8768d7351 243 extern int TMR2_COUNT_FLOWRATE;
Lightvalve 11:82d8768d7351 244 extern int TMR2_COUNT_DEADZONE;
Lightvalve 11:82d8768d7351 245 extern int TMR2_COUNT_PRES_NULL;
Lightvalve 11:82d8768d7351 246 extern int TMR2_COUNT_TORQUE_NULL;
Lightvalve 11:82d8768d7351 247 extern int TMR2_COUNT_PRES_CALIB;
Lightvalve 11:82d8768d7351 248 extern int TMR2_COUNT_REFERENCE;
Lightvalve 11:82d8768d7351 249 extern int TMR2_COUNT_JOINT;
Lightvalve 11:82d8768d7351 250 extern int TMR2_COUNT_ROTARY_FRIC_TUNE;
Lightvalve 11:82d8768d7351 251
Lightvalve 11:82d8768d7351 252 extern bool FLAG_REFERENCE_VALVE_PWM;
Lightvalve 11:82d8768d7351 253 extern bool FLAG_REFERENCE_VALVE_POSITION;
Lightvalve 11:82d8768d7351 254 extern bool FLAG_REFERENCE_JOINT_POSITION;
Lightvalve 11:82d8768d7351 255 extern bool FLAG_REFERENCE_JOINT_TORQUE;
Lightvalve 11:82d8768d7351 256 extern bool FLAG_REFERENCE_PRES_DIFF;
Lightvalve 11:82d8768d7351 257
Lightvalve 11:82d8768d7351 258 extern double TUNING_TIME;
Lightvalve 11:82d8768d7351 259
Lightvalve 11:82d8768d7351 260 extern double REFERENCE_FREQ;
Lightvalve 11:82d8768d7351 261 extern double REFERENCE_MAG;
Lightvalve 11:82d8768d7351 262
Lightvalve 11:82d8768d7351 263 extern bool FLAG_FIND_HOME;
Lightvalve 11:82d8768d7351 264
Lightvalve 11:82d8768d7351 265 extern int MODE_JUMP_STATUS;
Lightvalve 11:82d8768d7351 266