setting.h

00001 #include "mbed.h"
00002 #include "FastPWM.h"
00003 
00004 // pwm 
00005 #define PIN_V       PB_7
00006 #define PIN_W       PB_6
00007 //#define PWM_ARR 0x465         // loop 80k, pwm 40k 
00008 //#define PWM_ARR     0x8CA       // loop 40k, pwm 20k
00009 #define PWM_ARR     0x1194      // loop 20k, pwm 10k
00010 //#define PWM_ARR     0x2328    // loop 10k, pwm 5k
00011 #define TMR3_COUNT  0x4650      // loop 5k
00012 #define TMR2_COUNT  0x2710      // loop 500hz with prescale 18
00013 
00014 #define FREQ_500    500.0f
00015 #define FREQ_1k     1000.0f
00016 #define FREQ_5k     5000.0f
00017 #define FREQ_10k    10000.0f
00018 #define FREQ_20k    20000.0f
00019 #define FREQ_40k    40000.0f
00020 #define DT_500      0.002f
00021 #define DT_1k       0.0005f
00022 #define DT_5k       0.0002f
00023 #define DT_10k      0.0001f
00024 #define DT_20k      0.00005f
00025 #define DT_40k      0.000025f
00026 
00027 extern DigitalOut check;
00028 extern DigitalOut check_2;
00029 extern AnalogOut dac_1;
00030 extern AnalogOut dac_2;
00031 
00032 extern float dtc_v;
00033 extern float dtc_w;
00034 
00035 // I2C
00036 extern I2C i2c; // SDA, SCL (for K22F)
00037 extern const int i2c_slave_addr1; 
00038 extern unsigned int value; // 10bit output of reading sensor AS5510
00039 
00040 // SPI
00041 extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK);
00042 extern DigitalOut eeprom_cs;
00043 extern SPI enc;
00044 extern DigitalOut enc_cs;
00045 
00046 // UART 
00047 extern Serial pc; //Serial pc(PA_9,PA_10); _ UART
00048 
00049 // CAN
00050 extern CAN can;
00051 extern CANMessage msg;
00052 
00053 // Board Information 
00054 
00055 
00056 // User Function 
00057 
00058 void CurrentControl();
00059 void ValveControl(unsigned int ControlMode);
00060 
00061 
00062 /*******************************************************************************
00063  * COMMON Settings
00064  ******************************************************************************/
00065 #define             LATEST_VERSION      19032
00066 
00067 
00068 
00069 /*******************************************************************************
00070  * COMMON CONSTANTS
00071  ******************************************************************************/
00072 #define             RAD_30              0.523598775598299f
00073 #define             RAD_60              1.047197551196598f
00074 #define             RAD_120             2.094395102393195f
00075 #define             RAD_180             3.141592653589793f
00076 #define             RAD_240             4.188790204786391f
00077 #define             RAD_300             5.235987755982989f
00078 #define             RAD_360             6.283185307179586f
00079 
00080 #define             SYSFREQ             200000000
00081 #define             PBCLK               100000000
00082 
00083 //#define             TMR_FREQ_10k       10000
00084 #define             TMR_FREQ_5k       5000
00085 
00086 #define             FALSE               0
00087 #define             TRUE                1
00088 #define             OUTPUT              0
00089 #define             INPUT               1
00090 #define             LOW                 0
00091 #define             HIGH                1
00092 #define             DIGITAL             0
00093 #define             ANALOG              1
00094 
00095 #define             PI                  3.141592653589793f
00096 #define             D2R                 0.017453292519943f
00097 #define             R2D                 57.295779513082323f
00098 
00099 
00100 /*******************************************************************************
00101  * HEADER INCLUDE
00102  ******************************************************************************/
00103 
00104 /*******************************************************************************
00105  * VARIABLE
00106  ******************************************************************************/
00107 
00108 // Board Information 
00109 extern uint8_t BNO;
00110 extern uint8_t CONTROL_MODE;
00111 extern uint8_t OPERATING_MODE;
00112 extern uint8_t SENSING_MODE;
00113 extern uint8_t CONTROL_UTILITY_MODE;
00114 extern uint8_t CURRENT_CONTROL_MODE;
00115 extern uint8_t FLAG_VALVE_DEADZONE;
00116 extern uint8_t REFERENCE_MODE;
00117 extern int16_t CAN_FREQ;
00118 extern int16_t DIR_JOINT_ENC;
00119 extern int16_t DIR_VALVE;
00120 extern int16_t DIR_VALVE_ENC;
00121 
00122 extern float SUPPLY_VOLTAGE;
00123 extern float VALVE_VOLTAGE_LIMIT;
00124 
00125 extern float P_GAIN_VALVE_POSITION;
00126 extern float I_GAIN_VALVE_POSITION;
00127 extern float D_GAIN_VALVE_POSITION;
00128 extern float P_GAIN_JOINT_POSITION;
00129 extern float I_GAIN_JOINT_POSITION;
00130 extern float D_GAIN_JOINT_POSITION;
00131 extern float P_GAIN_JOINT_TORQUE;
00132 extern float I_GAIN_JOINT_TORQUE;
00133 extern float D_GAIN_JOINT_TORQUE;
00134 
00135 extern float K_SPRING;
00136 extern float D_DAMPER;
00137 
00138 extern int16_t flag_delay_test;
00139 
00140 //extern float P_GAIN_VALVE_POSITION_OPP;
00141 //extern float I_GAIN_VALVE_POSITION_OPP;
00142 //extern float D_GAIN_VALVE_POSITION_OPP;
00143 //extern float P_GAIN_JOINT_POSITION_OPP;
00144 //extern float I_GAIN_JOINT_POSITION_OPP;
00145 //extern float D_GAIN_JOINT_POSITION_OPP;
00146 //extern float P_GAIN_JOINT_TORQUE_OPP;
00147 //extern float I_GAIN_JOINT_TORQUE_OPP;
00148 //extern float D_GAIN_JOINT_TORQUE_OPP;
00149 
00150 //extern float VALVE_DEADZONE_PLUS;
00151 //extern float VALVE_DEADZONE_MINUS;
00152 
00153 extern int16_t VELOCITY_COMP_GAIN;
00154 extern int16_t COMPLIANCE_GAIN;
00155 
00156 extern int16_t VALVE_CENTER;
00157 
00158 extern int16_t VALVE_FF;
00159 
00160 extern int16_t BULK_MODULUS;
00161 
00162 extern int16_t CHAMBER_VOLUME_A;
00163 extern int16_t CHAMBER_VOLUME_B;
00164 
00165 extern int16_t PISTON_AREA_A;
00166 extern int16_t PISTON_AREA_B;
00167 extern float PISTON_AREA_alpha;
00168 extern float alpha3;
00169 
00170 
00171 extern int16_t PRES_SUPPLY;
00172 extern int16_t PRES_RETURN;
00173 
00174 extern int16_t ENC_LIMIT_PLUS;
00175 extern int16_t ENC_LIMIT_MINUS;
00176 
00177 extern int16_t STROKE;
00178 
00179 
00180 //extern int16_t VALVE_LIMIT_PLUS;
00181 //extern int16_t VALVE_LIMIT_MINUS;
00182 
00183 extern float ENC_PULSE_PER_POSITION;
00184 extern float TORQUE_SENSOR_PULSE_PER_TORQUE;
00185 
00186 extern float PRES_SENSOR_A_PULSE_PER_BAR;
00187 extern float PRES_SENSOR_B_PULSE_PER_BAR;
00188 
00189 extern int HOMEPOS_OFFSET;
00190 extern int HOMEPOS_VALVE_OPENING;
00191 
00192 extern float FRICTION;
00193 extern float REF_PERIOD;
00194 extern float REF_MAG;
00195 extern int REF_NUM;
00196 
00197 
00198 extern float DAC_REF;
00199 extern float DAC_RESOL;
00200 
00201 extern int REF_POSITION;
00202 extern int REF_VELOCITY;
00203 extern int16_t REF_TORQUE;
00204 extern int16_t REF_PRES_DIFF;
00205 extern int16_t REF_PWM;
00206 extern int16_t REF_VALVE_POSITION;
00207 extern int16_t REF_CURRENT;
00208 
00209 extern int REF_MOVE_TIME_5k;
00210 extern int INIT_REF_PWM;
00211 extern int INIT_REF_VALVE_POS;
00212 extern int INIT_REF_POS;
00213 extern int INIT_REF_VEL;
00214 extern int INIT_REF_TORQUE;
00215 extern int INIT_REF_PRES_DIFF;
00216 extern int INIT_REF_CURRENT;
00217 
00218 extern int CUR_POSITION;
00219 extern int CUR_VELOCITY;
00220 extern float CUR_TORQUE;
00221 extern float CUR_PRES_A;
00222 extern float CUR_PRES_B;
00223 extern int CUR_VALVE_POSITION;
00224 
00225 extern unsigned int    TMR2_COUNT_LED1;
00226 extern unsigned int    TMR2_COUNT_LED2;
00227 extern unsigned int    TMR2_COUNT_CAN_TX;
00228 extern unsigned int    TMR3_COUNT_TEST;
00229 
00230 extern int num_err;
00231 extern int flag_err[8];
00232 extern int flag_err_old[8];
00233 extern int flag_err_rt;
00234 
00235 extern int flag_ref_enable;
00236 
00237 extern int flag_data_request[5];
00238 
00239 extern int MODE_POS_FT_TRANS;
00240 
00241 extern float CUR_CURRENT_mA;
00242 extern float CUR_PRES_A_BAR;
00243 extern float CUR_PRES_B_BAR;
00244 extern float CUR_TORQUE_NM;
00245 extern float CUR_TORQUE_NM_PRESS;
00246 
00247 extern float PRES_A_VREF;
00248 extern float PRES_B_VREF;
00249 extern float TORQUE_VREF;
00250 
00251 extern float VALVE_PWM_RAW_FB;
00252 extern float VALVE_PWM_RAW_FF;
00253 extern float VALVE_PWM_RAW;
00254 extern int VALVE_PWM_VALVE_DZ;
00255 
00256 extern float VALVE_GAIN_LPM_PER_V[10];
00257 extern float VALVE_POS_VS_PWM[25];
00258 extern long JOINT_VEL[100];
00259 
00260 extern int VALVE_MAX_POS;
00261 extern int VALVE_MIN_POS;
00262 extern int VALVE_POS_NUM;
00263 extern float VALVE_CENTER_OFFSET;
00264 extern float VALVE_DZ_MINUS_OFFSET;
00265 extern float VALVE_DZ_PLUS_OFFSET;
00266 extern float VALVE_DEADZONE_MINUS;
00267 extern float VALVE_DEADZONE_PLUS;
00268 
00269 extern int TMR3_COUNT_FINDHOME;
00270 extern int TMR3_COUNT_FLOWRATE;
00271 extern int TMR3_COUNT_DEADZONE;
00272 extern int TMR3_COUNT_PRES_NULL;
00273 extern int TMR3_COUNT_TORQUE_NULL;
00274 extern int TMR3_COUNT_PRES_CALIB;
00275 extern int TMR3_COUNT_REFERENCE;
00276 extern int TMR3_COUNT_JOINT;
00277 extern int TMR3_COUNT_ROTARY_FRIC_TUNE;
00278 
00279 extern float TUNING_TIME;
00280 
00281 extern float REFERENCE_FREQ;
00282 extern float REFERENCE_MAG;
00283 
00284 extern bool FLAG_FIND_HOME;
00285 
00286 extern int MODE_JUMP_STATUS;
00287 
00288 extern float CUR_PRES_DIFF_BAR;
00289 extern float CUR_PRES_A_sum;
00290 extern float CUR_PRES_B_sum;
00291 extern float CUR_PRES_A_mean;
00292 extern float CUR_PRES_B_mean;
00293 extern float CUR_TORQUE_sum;
00294 extern float CUR_TORQUE_mean;
00295 extern float PRES_A_NULL;
00296 extern float PRES_B_NULL;
00297 extern float TORQUE_NULL;
00298 
00299 extern float Ref_Valve_Pos_Old;
00300 
00301 extern int VALVE_ID_timer;
00302 extern int VALVE_DZ_timer;
00303 extern int VALVE_FR_timer;
00304 extern int VALVE_HPL_timer;
00305 extern int VALVE_POS_TMP;
00306 extern int JOINT_VEL_TMP;
00307 extern int DDV_POS_AVG ;
00308 extern int VALVE_POS_AVG[50];
00309 extern int VALVE_POS_AVG_OLD;
00310 extern int data_num;
00311 extern int ID_index;
00312 extern int DZ_index;
00313 extern int ID_index_array[50];
00314 extern int first_check;
00315 extern float init_time;
00316 extern int DZ_case;
00317 extern int START_POS;
00318 extern int FINAL_POS;
00319 extern int DZ_DIRECTION;
00320 extern int FIRST_DZ;
00321 extern int SECOND_DZ;
00322 extern int DZ_NUM;
00323 extern int one_period_end;
00324 extern float Ref_Vel_Test;
00325 extern long TMR2_FOR_SLOW_LOGGING;
00326 //extern int velcount;
00327 extern char max_check;
00328 extern char min_check; 
00329 
00330 extern float valve_pos_err, valve_pos_err_old, valve_pos_err_diff, valve_pos_err_sum;
00331 extern float joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_diff_fil, joint_pos_err_sum;
00332 extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum;
00333 extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ;
00334 
00335 extern float CUR_FLOWRATE;
00336 extern float VALVE_FF_VOLTAGE;
00337 
00338 extern int pos_plus_end;
00339 extern int pos_minus_end;
00340 
00341 extern bool need_enc_init;
00342 
00343 extern int temp_time;
00344 
00345 extern float CUR_VELOCITY_sum;
00346 extern float temp_vel_sum;
00347 
00348 extern int DZ_dir;
00349 extern int DZ_temp_cnt;
00350 extern int DZ_temp_cnt2;
00351 extern int DZ_end;
00352 extern int flag_flowrate;
00353 extern int fl_temp_cnt;
00354 extern int fl_temp_cnt2;
00355 extern int cur_vel_sum;
00356 
00357 extern float Cur_Valve_Open_pulse;
00358 
00359 // find home
00360 extern int CUR_VELOCITY_OLD;
00361 extern int cnt_findhome;
00362 extern int cnt_vel_findhome;
00363 extern int cnt_findhome_wait;
00364 extern int FINDHOME_VELOCITY;
00365 extern int FINDHOME_VELOCITY_OLD;
00366 extern int FINDHOME_POSITION;
00367 extern int FINDHOME_POSITION_OLD;
00368 
00369 // valve gain
00370 extern int check_vel_pos_init;
00371 extern int check_vel_pos_fin;
00372 extern int check_vel_pos_interv;
00373 extern int valve_gain_repeat_cnt;
00374 extern float VALVE_VOLTAGE;
00375 
00376 extern float freq_fric_tune;
00377 
00378 extern uint32_t TMR3_COUNT_CAN_TX;
00379 
00380 // Current Control Variables
00381 extern double I_REF;
00382 extern double I_REF_fil;
00383 extern double I_ERR;
00384 extern double I_ERR_INT;
00385 extern double I_REF_fil_old;
00386 extern double I_REF_fil_diff;
00387 
00388 // system id
00389 extern int cnt_sysid;
00390 extern double freq_sysid_Iref;
00391 
00392 extern int TMR3_COUNT_IREF;
00393 extern float CUR_CURRENT;
00394 extern float u_CUR[3];
00395 
00396 extern int FINDHOME_STAGE;
00397 extern int FINDHOME_INIT;
00398 extern int FINDHOME_GOTOLIMIT;
00399 extern int FINDHOME_ZEROPOSE;
00400 extern int FINDHOME_WAIT;
00401 
00402 extern float alpha_trans;
00403 
00404 
00405 extern float V_out;
00406 extern float V_rem;
00407 extern float V_MAX;
00408 
00409 extern float PWM_out;
00410 
00411 extern double K_v;
00412 extern double mV_PER_mA;
00413 extern double mV_PER_pulse;
00414 extern double mA_PER_pulse;
00415 
00416 extern int timer_while;
00417 extern int while_index;
00418 
00419 extern float K_LPF;
00420 extern float D_LPF;
00421 
00422 
00423 
00424 
00425 
00426 
00427 
00428 
00429 
00430 
00431 
00432 
00433 
00434 
00435 
00436 
00437 
00438 
00439