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