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