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 void CurrentControl();
00058 void ValveControl(unsigned int ControlMode);
00059 
00060 
00061 /*******************************************************************************
00062  * COMMON Settings
00063  ******************************************************************************/
00064 #define             LATEST_VERSION      19032
00065 
00066 
00067 /*******************************************************************************
00068  * COMMON CONSTANTS
00069  ******************************************************************************/
00070 #define             RAD_30              0.523598775598299f
00071 #define             RAD_60              1.047197551196598f
00072 #define             RAD_120             2.094395102393195f
00073 #define             RAD_180             3.141592653589793f
00074 #define             RAD_240             4.188790204786391f
00075 #define             RAD_300             5.235987755982989f
00076 #define             RAD_360             6.283185307179586f
00077 
00078 #define             SYSFREQ             200000000
00079 #define             PBCLK               100000000
00080 
00081 //#define             TMR_FREQ_10k       10000
00082 #define             TMR_FREQ_5k       5000
00083 
00084 #define             FALSE               0
00085 #define             TRUE                1
00086 #define             OUTPUT              0
00087 #define             INPUT               1
00088 #define             LOW                 0
00089 #define             HIGH                1
00090 #define             DIGITAL             0
00091 #define             ANALOG              1
00092 
00093 #define             PI                  3.141592653589793f
00094 #define             D2R                 0.017453292519943f
00095 #define             R2D                 57.295779513082323f
00096 
00097 
00098 /*******************************************************************************
00099  * HEADER INCLUDE
00100  ******************************************************************************/
00101 
00102 /*******************************************************************************
00103  * VARIABLE
00104  ******************************************************************************/
00105 
00106 // Board Information 
00107 extern uint8_t BNO;
00108 extern uint8_t CONTROL_MODE;
00109 extern uint8_t OPERATING_MODE;
00110 extern uint8_t SENSING_MODE;
00111 extern uint8_t CONTROL_UTILITY_MODE;
00112 extern uint8_t CURRENT_CONTROL_MODE;
00113 extern uint8_t FLAG_VALVE_DEADZONE;
00114 extern uint8_t REFERENCE_MODE;
00115 extern int16_t CAN_FREQ;
00116 extern int16_t DIR_JOINT_ENC;
00117 extern int16_t DIR_VALVE;
00118 extern int16_t DIR_VALVE_ENC;
00119 
00120 extern float SUPPLY_VOLTAGE;
00121 extern float VALVE_VOLTAGE_LIMIT;
00122 
00123 extern float P_GAIN_VALVE_POSITION;
00124 extern float I_GAIN_VALVE_POSITION;
00125 extern float D_GAIN_VALVE_POSITION;
00126 extern float P_GAIN_JOINT_POSITION;
00127 extern float I_GAIN_JOINT_POSITION;
00128 extern float D_GAIN_JOINT_POSITION;
00129 extern float P_GAIN_JOINT_TORQUE;
00130 extern float I_GAIN_JOINT_TORQUE;
00131 extern float D_GAIN_JOINT_TORQUE;
00132 extern float P_GAIN_JOINT_TORQUE_FF;
00133 extern float I_GAIN_JOINT_TORQUE_FF;
00134 extern float D_GAIN_JOINT_TORQUE_FF;
00135 
00136 extern int16_t K_SPRING;
00137 extern int16_t D_DAMPER;
00138 
00139 extern int16_t flag_delay_test;
00140 
00141 //extern float P_GAIN_VALVE_POSITION_OPP;
00142 //extern float I_GAIN_VALVE_POSITION_OPP;
00143 //extern float D_GAIN_VALVE_POSITION_OPP;
00144 //extern float P_GAIN_JOINT_POSITION_OPP;
00145 //extern float I_GAIN_JOINT_POSITION_OPP;
00146 //extern float D_GAIN_JOINT_POSITION_OPP;
00147 //extern float P_GAIN_JOINT_TORQUE_OPP;
00148 //extern float I_GAIN_JOINT_TORQUE_OPP;
00149 //extern float D_GAIN_JOINT_TORQUE_OPP;
00150 
00151 extern float VALVE_DEADZONE_PLUS;
00152 extern float VALVE_DEADZONE_MINUS;
00153 
00154 extern int16_t VELOCITY_COMP_GAIN;
00155 extern int16_t COMPLIANCE_GAIN;
00156 
00157 extern int16_t VALVE_CENTER;
00158 
00159 extern int16_t VALVE_FF;
00160 
00161 extern int16_t BULK_MODULUS;
00162 
00163 extern int16_t CHAMBER_VOLUME_A;
00164 extern int16_t CHAMBER_VOLUME_B;
00165 
00166 extern int16_t PISTON_AREA_A;
00167 extern int16_t PISTON_AREA_B;
00168 extern float PISTON_AREA_alpha;
00169 extern float alpha3;
00170 
00171 
00172 extern int16_t PRES_SUPPLY;
00173 extern int16_t PRES_RETURN;
00174 
00175 extern int16_t ENC_LIMIT_PLUS;
00176 extern int16_t ENC_LIMIT_MINUS;
00177 
00178 extern int16_t STROKE;
00179 
00180 
00181 extern float Amm;
00182 extern float beta;
00183 extern float Ps;
00184 extern float Pt;
00185 extern float gamma_hat;
00186 extern float a_hat;
00187 extern float V_adapt;
00188 extern float x_4_des_old;
00189 
00190 //extern int16_t VALVE_LIMIT_PLUS;
00191 //extern int16_t VALVE_LIMIT_MINUS;
00192 
00193 extern float ENC_PULSE_PER_POSITION;
00194 extern float TORQUE_SENSOR_PULSE_PER_TORQUE;
00195 
00196 extern float PRES_SENSOR_A_PULSE_PER_BAR;
00197 extern float PRES_SENSOR_B_PULSE_PER_BAR;
00198 
00199 extern int HOMEPOS_OFFSET;
00200 extern int HOMEPOS_VALVE_OPENING;
00201 
00202 extern float FRICTION;
00203 extern float REF_PERIOD;
00204 extern float REF_MAG;
00205 extern int REF_NUM;
00206 
00207 
00208 extern float DAC_REF;
00209 extern float DAC_RESOL;
00210 
00211 extern int REF_POSITION;
00212 extern int REF_VELOCITY;
00213 extern int16_t REF_TORQUE;
00214 extern int16_t REF_PRES_DIFF;
00215 extern int16_t REF_PWM;
00216 extern int16_t REF_VALVE_POSITION;
00217 extern int16_t REF_CURRENT;
00218 
00219 extern int REF_MOVE_TIME_5k;
00220 extern int INIT_REF_PWM;
00221 extern int INIT_REF_VALVE_POS;
00222 extern int INIT_REF_POS;
00223 extern int INIT_REF_VEL;
00224 extern int INIT_REF_TORQUE;
00225 extern int INIT_REF_PRES_DIFF;
00226 extern int INIT_REF_CURRENT;
00227 
00228 extern int CUR_POSITION;
00229 extern int CUR_VELOCITY;
00230 extern float CUR_TORQUE;
00231 extern float CUR_PRES_A;
00232 extern float CUR_PRES_B;
00233 extern int CUR_VALVE_POSITION;
00234 
00235 extern unsigned int    TMR2_COUNT_LED1;
00236 extern unsigned int    TMR2_COUNT_LED2;
00237 extern unsigned int    TMR2_COUNT_CAN_TX;
00238 extern unsigned int    TMR3_COUNT_TEST;
00239 
00240 extern int num_err;
00241 extern int flag_err[8];
00242 extern int flag_err_old[8];
00243 extern int flag_err_rt;
00244 
00245 extern int flag_ref_enable;
00246 
00247 extern int flag_data_request[5];
00248 
00249 extern int MODE_POS_FT_TRANS;
00250 extern int NN_Control_Flag;
00251 
00252 extern int cnt_buffer;
00253 
00254 extern float CUR_CURRENT_mA;
00255 extern float CUR_PRES_A_BAR;
00256 extern float CUR_PRES_B_BAR;
00257 extern float CUR_TORQUE_NM;
00258 extern float CUR_TORQUE_NM_PRESS;
00259 
00260 extern float PRES_A_VREF;
00261 extern float PRES_B_VREF;
00262 extern float TORQUE_VREF;
00263 
00264 extern float VALVE_PWM_RAW_FB;
00265 extern float VALVE_PWM_RAW_FF;
00266 extern float VALVE_PWM_RAW;
00267 extern int VALVE_PWM_VALVE_DZ;
00268 
00269 extern float VALVE_GAIN_LPM_PER_V[10];
00270 extern float VALVE_POS_VS_PWM[25];
00271 extern long JOINT_VEL[100];
00272 
00273 extern int VALVE_MAX_POS;
00274 extern int VALVE_MIN_POS;
00275 extern int VALVE_POS_NUM;
00276 extern float VALVE_CENTER_OFFSET;
00277 extern float VALVE_DZ_MINUS_OFFSET;
00278 extern float VALVE_DZ_PLUS_OFFSET;
00279 extern float VALVE_DEADZONE_MINUS;
00280 extern float VALVE_DEADZONE_PLUS;
00281 
00282 extern int TMR3_COUNT_FINDHOME;
00283 extern int TMR3_COUNT_FLOWRATE;
00284 extern int TMR3_COUNT_DEADZONE;
00285 extern int TMR3_COUNT_PRES_NULL;
00286 extern int TMR3_COUNT_TORQUE_NULL;
00287 extern int TMR3_COUNT_PRES_CALIB;
00288 extern int TMR3_COUNT_REFERENCE;
00289 extern int TMR3_COUNT_JOINT;
00290 extern int TMR3_COUNT_ROTARY_FRIC_TUNE;
00291 
00292 extern float TUNING_TIME;
00293 
00294 extern float REFERENCE_FREQ;
00295 extern float REFERENCE_MAG;
00296 
00297 extern bool FLAG_FIND_HOME;
00298 
00299 extern int MODE_JUMP_STATUS;
00300 
00301 extern float CUR_PRES_DIFF_BAR;
00302 extern float CUR_PRES_A_sum;
00303 extern float CUR_PRES_B_sum;
00304 extern float CUR_PRES_A_mean;
00305 extern float CUR_PRES_B_mean;
00306 extern float CUR_TORQUE_sum;
00307 extern float CUR_TORQUE_mean;
00308 extern float PRES_A_NULL;
00309 extern float PRES_B_NULL;
00310 extern float TORQUE_NULL;
00311 
00312 extern float Ref_Valve_Pos_Old;
00313 
00314 extern int VALVE_ID_timer;
00315 extern int VALVE_DZ_timer;
00316 extern int VALVE_FR_timer;
00317 extern int VALVE_HPL_timer;
00318 extern int VALVE_POS_TMP;
00319 extern int JOINT_VEL_TMP;
00320 extern int DDV_POS_AVG ;
00321 extern int VALVE_POS_AVG[50];
00322 extern int VALVE_POS_AVG_OLD;
00323 extern int data_num;
00324 extern int ID_index;
00325 extern int DZ_index;
00326 extern int ID_index_array[50];
00327 extern int first_check;
00328 extern float init_time;
00329 extern int DZ_case;
00330 extern int START_POS;
00331 extern int FINAL_POS;
00332 extern int DZ_DIRECTION;
00333 extern int FIRST_DZ;
00334 extern int SECOND_DZ;
00335 extern int DZ_NUM;
00336 extern int one_period_end;
00337 extern float Ref_Vel_Test;
00338 extern long TMR2_FOR_SLOW_LOGGING;
00339 //extern int velcount;
00340 extern char max_check;
00341 extern char min_check; 
00342 
00343 extern float valve_pos_err, valve_pos_err_old, valve_pos_err_diff, valve_pos_err_sum;
00344 extern float joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_diff_fil, joint_pos_err_sum;
00345 extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum;
00346 extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ;
00347 
00348 extern float CUR_FLOWRATE;
00349 extern float VALVE_FF_VOLTAGE;
00350 
00351 extern int pos_plus_end;
00352 extern int pos_minus_end;
00353 
00354 extern bool need_enc_init;
00355 
00356 extern int temp_time;
00357 
00358 extern float CUR_VELOCITY_sum;
00359 extern float temp_vel_sum;
00360 
00361 extern int DZ_dir;
00362 extern int DZ_temp_cnt;
00363 extern int DZ_temp_cnt2;
00364 extern int DZ_end;
00365 extern int flag_flowrate;
00366 extern int fl_temp_cnt;
00367 extern int fl_temp_cnt2;
00368 extern int cur_vel_sum;
00369 
00370 extern float Cur_Valve_Open_pulse;
00371 
00372 // find home
00373 extern int CUR_VELOCITY_OLD;
00374 extern int cnt_findhome;
00375 extern int cnt_vel_findhome;
00376 extern int FINDHOME_VELOCITY;
00377 extern int FINDHOME_VELOCITY_OLD;
00378 extern int FINDHOME_POSITION;
00379 extern int FINDHOME_POSITION_OLD;
00380 
00381 // valve gain
00382 extern int check_vel_pos_init;
00383 extern int check_vel_pos_fin;
00384 extern int check_vel_pos_interv;
00385 extern int valve_gain_repeat_cnt;
00386 extern float VALVE_VOLTAGE;
00387 
00388 extern float freq_fric_tune;
00389 
00390 extern uint32_t TMR3_COUNT_CAN_TX;
00391 
00392 // Current Control Variables
00393 extern double I_REF;
00394 extern double I_REF_fil;
00395 extern double I_ERR;
00396 extern double I_ERR_INT;
00397 extern double I_REF_fil_old;
00398 extern double I_REF_fil_diff;
00399 
00400 // system id
00401 extern int cnt_sysid;
00402 extern double freq_sysid_Iref;
00403 
00404 extern int cnt_freq_test;
00405 extern int cnt_step_test;
00406 extern int buffer_data_size;
00407 extern int cnt_send_buffer;
00408 extern float freq_test_valve_ref;
00409 extern float ref_array[10000];
00410 extern int pos_array[10000];
00411 extern int flag_every_reference;
00412 
00413 extern int TMR3_COUNT_IREF;
00414 extern float CUR_CURRENT;
00415 extern float u_CUR[3];
00416 
00417 extern int FINDHOME_STAGE;
00418 extern int FINDHOME_INIT;
00419 extern int FINDHOME_GOTOLIMIT;
00420 extern int FINDHOME_ZEROPOSE;
00421 
00422 extern float alpha_trans;
00423 
00424 
00425 extern float V_out;
00426 extern float V_rem;
00427 extern float V_MAX;
00428 
00429 extern float PWM_out;
00430 
00431 extern double K_v;
00432 extern double mV_PER_mA;
00433 extern double mV_PER_pulse;
00434 extern double mA_PER_pulse;
00435 
00436 extern int timer_while;
00437 extern int while_index;
00438 extern int RL_timer;
00439 
00440 extern float K_LPF;
00441 extern float D_LPF;
00442 
00443 extern float torq_sen_past;
00444 extern float torq_ref_past;
00445 extern float output_normalized;
00446 
00447 
00448 
00449 
00450 
00451 
00452 
00453 
00454 
00455 
00456 
00457