Kim GiJeong
eeprom_test
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //200518 00002 #include "mbed.h" 00003 #include "FastPWM.h" 00004 #include "INIT_HW.h" 00005 #include "function_CAN.h" 00006 #include "SPI_EEP_ENC.h" 00007 #include "I2C_AS5510.h" 00008 #include "setting.h" 00009 #include "function_utilities.h" 00010 #include "stm32f4xx_flash.h" 00011 #include "FlashWriter.h" 00012 00013 ///191008//// 00014 00015 // dac & check /////////////////////////////////////////// 00016 DigitalOut check(PC_2); 00017 DigitalOut check_2(PC_3); 00018 AnalogOut dac_1(PA_4); 00019 AnalogOut dac_2(PA_5); 00020 AnalogIn adc1(PC_4); //pressure_1 00021 AnalogIn adc2(PB_0); //pressure_2 00022 AnalogIn adc3(PC_1); //current 00023 00024 00025 // PWM /////////////////////////////////////////// 00026 float dtc_v=0.0f; 00027 float dtc_w=0.0f; 00028 00029 // I2C /////////////////////////////////////////// 00030 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F) 00031 const int i2c_slave_addr1 = 0x56; 00032 unsigned int value; // 10bit output of reading sensor AS5510 00033 00034 // SPI /////////////////////////////////////////// 00035 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK); 00036 DigitalOut eeprom_cs(PB_12); 00037 //FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector 00038 SPI enc(PC_12,PC_11,PC_10); 00039 DigitalOut enc_cs(PD_2); 00040 DigitalOut LED(PA_15); 00041 00042 // UART /////////////////////////////////////////// 00043 Serial pc(PA_9,PA_10); // _ UART 00044 00045 // CAN /////////////////////////////////////////// 00046 CAN can(PB_8, PB_9, 1000000); 00047 CANMessage msg; 00048 void onMsgReceived() 00049 { 00050 CAN_RX_HANDLER(); 00051 } 00052 00053 // Variables /////////////////////////////////////////// 00054 State pos; 00055 State vel; 00056 State Vout; 00057 State torq; 00058 State pres_A; 00059 State pres_B; 00060 State cur; 00061 State valve_pos; 00062 00063 State INIT_Vout; 00064 State INIT_Valve_Pos; 00065 State INIT_Pos; 00066 State INIT_torq; 00067 00068 extern int CID_RX_CMD; 00069 extern int CID_RX_REF_POSITION; 00070 extern int CID_RX_REF_VALVE_POS; 00071 extern int CID_RX_REF_PWM; 00072 00073 extern int CID_TX_INFO; 00074 extern int CID_TX_POSITION; 00075 extern int CID_TX_TORQUE; 00076 extern int CID_TX_PRES; 00077 extern int CID_TX_VOUT; 00078 extern int CID_TX_VALVE_POSITION; 00079 00080 float I_REF_fil_DZ = 0.0f; 00081 00082 // ============================================================================= 00083 // ============================================================================= 00084 // ============================================================================= 00085 00086 /******************************************************************************* 00087 * REFERENCE MODE 00088 ******************************************************************************/ 00089 enum _REFERENCE_MODE { 00090 MODE_REF_NO_ACT = 0, //0 00091 MODE_REF_DIRECT, //1 00092 MODE_REF_COS_INC, //2 00093 MODE_REF_LINE_INC, //3 00094 MODE_REF_SIN_WAVE, //4 00095 MODE_REF_SQUARE_WAVE, //5 00096 }; 00097 00098 /******************************************************************************* 00099 * CONTROL MODE 00100 ******************************************************************************/ 00101 enum _CONTROL_MODE { 00102 //control mode 00103 MODE_NO_ACT = 0, //0 00104 MODE_VALVE_POSITION_CONTROL, //1 00105 MODE_JOINT_CONTROL, //2 00106 00107 MODE_VALVE_OPEN_LOOP, //3 00108 MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4 00109 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5 00110 00111 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6 00112 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7 00113 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8 00114 00115 MODE_TEST_CURRENT_CONTROL, //9 00116 MODE_TEST_PWM_CONTROL, //10 00117 00118 MODE_CURRENT_CONTROL, //11 00119 MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12 00120 MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13 00121 00122 //utility 00123 MODE_TORQUE_SENSOR_NULLING = 20, //20 00124 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21 00125 MODE_FIND_HOME, //22 00126 MODE_VALVE_GAIN_SETTING, //23 00127 MODE_PRESSURE_SENSOR_NULLING, //24 00128 MODE_PRESSURE_SENSOR_CALIB, //25 00129 MODE_ROTARY_FRICTION_TUNING, //26 00130 00131 MODE_DDV_POS_VS_PWM_ID = 30, //30 00132 MODE_DDV_DEADZONE_AND_CENTER, //31 00133 MODE_DDV_POS_VS_FLOWRATE, //32 00134 MODE_SYSTEM_ID, //33 00135 }; 00136 00137 void SystemClock_Config(void); 00138 00139 int main() 00140 { 00141 /********************************* 00142 *** Initialization 00143 *********************************/ 00144 HAL_Init(); 00145 // SystemClock_Config(); 00146 00147 00148 // LED = 1; 00149 // pc.baud(9600); 00150 00151 // i2c init 00152 i2c.frequency(400 * 1000); // 0.4 mHz 00153 wait_ms(2); // Power Up wait 00154 look_for_hardware_i2c(); // Hardware present 00155 init_as5510(i2c_slave_addr1); 00156 make_delay(); 00157 00158 // // spi init 00159 //eeprom.format(8,3); 00160 //eeprom.frequency(5000000); //5M 00161 enc.format(8,0); 00162 enc.frequency(5000000); //5M 00163 make_delay(); 00164 00165 //rom 00166 //ROM_CALL_DATA(); 00167 spi_eeprom_call_data(); 00168 make_delay(); 00169 00170 // ADC init 00171 Init_ADC(); 00172 make_delay(); 00173 00174 // Pwm init 00175 Init_PWM(); 00176 TIM4->CR1 ^= TIM_CR1_UDIS; 00177 make_delay(); 00178 00179 // TMR3 init 00180 Init_TMR3(); 00181 TIM3->CR1 ^= TIM_CR1_UDIS; 00182 make_delay(); 00183 00184 // TMR2 init 00185 // Init_TMR2(); 00186 // TIM2->CR1 ^= TIM_CR1_UDIS; 00187 // make_delay(); 00188 00189 // CAN 00190 // can.reset(); 00191 CAN_ID_INIT(); 00192 // can.filter(msg.id, 0xFFFFF000, CANStandard); 00193 // can.frequency(1000000); // set CAN bit rate to 1Mbps 00194 can.filter(CID_RX_CMD, 0xFFF, CANStandard, 0); 00195 can.filter(CID_RX_REF_POSITION, 0xFFF, CANStandard, 1); 00196 can.filter(CID_RX_REF_VALVE_POS, 0xFFF, CANStandard, 2); 00197 can.filter(CID_RX_REF_PWM, 0xFFF, CANStandard, 3); 00198 can.attach(&CAN_RX_HANDLER); 00199 00200 make_delay(); 00201 00202 //Timer priority 00203 NVIC_SetPriority(TIM3_IRQn, 2); 00204 //NVIC_SetPriority(TIM2_IRQn, 3); 00205 NVIC_SetPriority(TIM4_IRQn, 3); 00206 00207 00208 00209 // spi _ enc 00210 spi_enc_set_init(); 00211 make_delay(); 00212 00213 //DAC init 00214 if (SENSING_MODE == 0) { 00215 dac_1 = TORQUE_VREF / 3.3f; 00216 dac_2 = 0.0f; 00217 } else if (SENSING_MODE == 1) { 00218 dac_1 = PRES_A_VREF / 3.3f; 00219 dac_2 = PRES_B_VREF / 3.3f; 00220 } 00221 make_delay(); 00222 00223 for (int i=0; i<50; i++) { 00224 if(i%2==0) 00225 ID_index_array[i] = - i * 0.5f; 00226 else 00227 ID_index_array[i] = (i+1) * 0.5f; 00228 } 00229 00230 /************************************ 00231 *** Program is operating! 00232 *************************************/ 00233 while(1) { 00234 if(timer_while==1000 && OPERATING_MODE==5) { 00235 // if(timer_while==1000) { 00236 //i2c 00237 read_field(i2c_slave_addr1); 00238 if(DIR_VALVE_ENC < 0) value = 1023 - value; 00239 // if(LED==1) { 00240 // LED=0; 00241 // } else 00242 // LED = 1; 00243 timer_while = 0; 00244 } 00245 timer_while ++; 00246 //pc.printf("%d\n", value); 00247 } 00248 } 00249 00250 float DDV_JOINT_POS_FF(float REF_JOINT_VEL) 00251 { 00252 00253 int i = 0; 00254 float Ref_Valve_Pos_FF = 0.0f; 00255 for(i=0; i<VALVE_POS_NUM; i++) { 00256 if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) { 00257 if(i==0) { 00258 if(JOINT_VEL[i+1] == JOINT_VEL[i]) { 00259 Ref_Valve_Pos_FF = (float) VALVE_CENTER; 00260 } else { 00261 Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER; 00262 } 00263 } else { 00264 if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) { 00265 Ref_Valve_Pos_FF = (float) VALVE_CENTER; 00266 } else { 00267 Ref_Valve_Pos_FF = ((float) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + (float) VALVE_CENTER + (float) (10*ID_index_array[i-1]); 00268 } 00269 } 00270 break; 00271 } 00272 } 00273 if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { 00274 Ref_Valve_Pos_FF = (float) VALVE_MAX_POS; 00275 } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { 00276 Ref_Valve_Pos_FF = (float) VALVE_MIN_POS; 00277 } 00278 00279 Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER); 00280 return Ref_Valve_Pos_FF; 00281 00282 } 00283 00284 00285 void VALVE_POS_CONTROL(float REF_VALVE_POS) 00286 { 00287 int i = 0; 00288 00289 if(REF_VALVE_POS > VALVE_MAX_POS) { 00290 REF_VALVE_POS = VALVE_MAX_POS; 00291 } else if(REF_VALVE_POS < VALVE_MIN_POS) { 00292 REF_VALVE_POS = VALVE_MIN_POS; 00293 } 00294 00295 valve_pos_err = REF_VALVE_POS - value; 00296 valve_pos_err_diff = valve_pos_err - valve_pos_err_old; 00297 valve_pos_err_old = valve_pos_err; 00298 valve_pos_err_sum += valve_pos_err; 00299 if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000; 00300 if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000; 00301 00302 VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff; 00303 00304 for(i=0; i<24; i++) { 00305 if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { 00306 if(i==0) { 00307 VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i]); 00308 } else { 00309 VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1]; 00310 } 00311 break; 00312 } 00313 } 00314 Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; 00315 } 00316 00317 #define LT_MAX_IDX 57 00318 float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f, 00319 -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f, 00320 -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f, 00321 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f, 00322 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f, 00323 25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f 00324 }; // duty 00325 float LT_Voltage_Output[LT_MAX_IDX] = {-230.0f, -215.0f, -192.5f, -185.0f, -177.5f, -170.0f, -164.0f, -160.0f, -150.0f, 00326 -150.0f, -145.0f, -145.0f, -145.0f, -135.0f, -135.0f, -135.0f, -127.5f, -127.5f, -115.0f, 00327 -115.0f, -115.0F, -100.0f, -100.0f, -100.0f, -60.0f, -60.0f, -10.0f, -5.0f, 0.0f, 00328 7.5f, 14.0f, 14.0f, 14.0f, 42.5f, 42.5f, 42.5f, 80.0f, 80.0f, 105.0f, 00329 105.0f, 105.0f, 120.0f, 120.0f, 120.0f, 131.0f, 131.0f, 140.0f, 140.0f, 140.0f, 00330 155.0f, 160.0f, 170.0f, 174.0f, 182.0f, 191.0f, 212.0f, 230.0f 00331 }; // mV 00332 00333 float PWM_duty_byLT(float Ref_V) 00334 { 00335 float PWM_duty = 0.0f; 00336 if(Ref_V<LT_Voltage_Output[0]) { 00337 PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5f+LT_PWM_duty[0]; 00338 } else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) { 00339 PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5f+LT_PWM_duty[LT_MAX_IDX-1]; 00340 } else { 00341 int idx = 0; 00342 for(idx=0; idx<LT_MAX_IDX-1; idx++) { 00343 float ini_x = LT_Voltage_Output[idx]; 00344 float fin_x = LT_Voltage_Output[idx+1]; 00345 float ini_y = LT_PWM_duty[idx]; 00346 float fin_y = LT_PWM_duty[idx+1]; 00347 if(Ref_V>=ini_x && Ref_V<fin_x) { 00348 PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y; 00349 break; 00350 } 00351 } 00352 } 00353 00354 return PWM_duty; 00355 } 00356 00357 00358 00359 00360 00361 /******************************************************************************* 00362 TIMER INTERRUPT 00363 *******************************************************************************/ 00364 00365 float FREQ_TMR4 = (float)FREQ_20k; 00366 float DT_TMR4 = (float)DT_20k; 00367 long CNT_TMR4 = 0; 00368 int TMR4_FREQ_10k = (int)FREQ_10k; 00369 extern "C" void TIM4_IRQHandler(void) 00370 { 00371 if (TIM4->SR & TIM_SR_UIF ) { 00372 00373 /******************************************************* 00374 *** Sensor Read & Data Handling 00375 ********************************************************/ 00376 00377 //Encoder 00378 if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) { 00379 ENC_UPDATE(); 00380 } 00381 00382 ADC1->CR2 |= 0x40000000; 00383 if (SENSING_MODE == 0) { 00384 // Torque Sensing (0~210)bar ============================================= 00385 float pres_A_new = (((float) ADC1->DR) - 2047.5f); 00386 double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz 00387 pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new; 00388 torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE; 00389 00390 00391 // float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f)); 00392 //// float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR; 00393 // float pres_A_new = ((float)ADC1->DR); 00394 // pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); 00395 // torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047 00396 00397 00398 } else if (SENSING_MODE == 1) { 00399 // Pressure Sensing (0~210)bar ============================================= 00400 float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); 00401 float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); 00402 double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 500Hz 00403 pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new; 00404 pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new; 00405 CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR; 00406 CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR; 00407 00408 if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator 00409 torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm 00410 } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator 00411 torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N 00412 } 00413 } 00414 00415 // //Pressure sensor A 00416 // ADC1->CR2 |= 0x40000000; // adc _ 12bit 00417 // //while((ADC1->SR & 0b10)); 00418 // float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f)); 00419 // float pres_A_new = ((float)ADC1->DR); 00420 // pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); 00421 // torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later. 00422 // 00423 // 00424 // //Pressure sensor B 00425 // float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f)); 00426 // float pres_B_new = ((float)ADC2->DR); 00427 // pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B); 00428 // //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B; 00429 00430 00431 //Current 00432 //ADC3->CR2 |= 0x40000000; // adc _ 12bit 00433 //int raw_cur = ADC3->DR; 00434 //while((ADC3->SR & 0b10)); 00435 float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz 00436 float cur_new = ((float)ADC3->DR-2048.0f) / 4096.0f * 3.3f * 10.0f / 1.44f; // unit : mA 00437 cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur); 00438 //cur.sen = raw_cur; 00439 00440 CNT_TMR4++; 00441 } 00442 TIM4->SR = 0x0; // reset the status register 00443 } 00444 00445 00446 int j =0; 00447 float FREQ_TMR3 = (float)FREQ_5k; 00448 float DT_TMR3 = (float)DT_5k; 00449 int cnt_trans = 0; 00450 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f; 00451 int can_rest =0; 00452 00453 extern "C" void TIM3_IRQHandler(void) 00454 { 00455 if (TIM3->SR & TIM_SR_UIF ) { 00456 00457 if (((OPERATING_MODE&0b110)>>1) == 0) { 00458 K_v = 0.4f; // Moog (LPM >> mA) , 100bar 00459 mV_PER_mA = 500.0f; // 5000mV/10mA 00460 mV_PER_pulse = 0.5f; // 5000mV/10000pulse 00461 mA_PER_pulse = 0.001f; // 10mA/10000pulse 00462 } else if (((OPERATING_MODE&0b110)>>1) == 1) { 00463 K_v = 0.5f; // KNR (LPM >> mA) , 100bar 00464 mV_PER_mA = 166.6666f; // 5000mV/30mA 00465 mV_PER_pulse = 0.5f; // 5000mV/10000pulse 00466 mA_PER_pulse = 0.003f; // 30mA/10000pulse 00467 } 00468 00469 if(MODE_POS_FT_TRANS == 1) { 00470 alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; 00471 cnt_trans++; 00472 torq.err_sum = 0; 00473 if((float)cnt_trans * DT_TMR3 > 3.0f) 00474 MODE_POS_FT_TRANS = 2; 00475 } else if(MODE_POS_FT_TRANS == 3) { 00476 alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; 00477 cnt_trans++; 00478 torq.err_sum = 0; 00479 if((float) cnt_trans * DT_TMR3 > 3.0f ) 00480 MODE_POS_FT_TRANS = 0; 00481 } else if(MODE_POS_FT_TRANS == 2) { 00482 alpha_trans = 1.0f; 00483 cnt_trans = 0; 00484 } else { 00485 alpha_trans = 0.0f; 00486 cnt_trans = 0; 00487 } 00488 00489 00490 int UTILITY_MODE = 0; 00491 int CONTROL_MODE = 0; 00492 00493 if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) { 00494 UTILITY_MODE = CONTROL_UTILITY_MODE; 00495 CONTROL_MODE = MODE_NO_ACT; 00496 } else { 00497 CONTROL_MODE = CONTROL_UTILITY_MODE; 00498 UTILITY_MODE = MODE_NO_ACT; 00499 } 00500 00501 00502 00503 // UTILITY MODE ------------------------------------------------------------ 00504 00505 switch (UTILITY_MODE) { 00506 case MODE_NO_ACT: { 00507 break; 00508 } 00509 00510 case MODE_TORQUE_SENSOR_NULLING: { 00511 00512 if(SENSING_MODE == 0){ 00513 00514 // DAC Voltage reference set 00515 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { 00516 CUR_TORQUE_sum += torq.sen; 00517 00518 if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { 00519 CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f; 00520 CUR_TORQUE_sum = 0; 00521 00522 TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean); 00523 00524 if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f; 00525 if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f; 00526 00527 //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); 00528 dac_1 = TORQUE_VREF / 3.3f; 00529 } 00530 } else { 00531 CONTROL_UTILITY_MODE = MODE_NO_ACT; 00532 TMR3_COUNT_TORQUE_NULL = 0; 00533 CUR_TORQUE_sum = 0; 00534 CUR_TORQUE_mean = 0; 00535 00536 ROM_RESET_DATA(); 00537 00538 dac_1 = TORQUE_VREF / 3.3f; 00539 } 00540 } else { 00541 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { 00542 CUR_PRES_A_sum += pres_A.sen; 00543 CUR_PRES_B_sum += pres_B.sen; 00544 00545 if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { 00546 CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f; 00547 CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f; 00548 CUR_PRES_A_sum = 0; 00549 CUR_PRES_B_sum = 0; 00550 00551 float VREF_NullingGain = 0.0003f; 00552 PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean; 00553 PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean; 00554 00555 if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; 00556 if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; 00557 if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; 00558 if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; 00559 00560 dac_1 = PRES_A_VREF / 3.3f; 00561 dac_2 = PRES_B_VREF / 3.3f; 00562 } 00563 } else { 00564 CONTROL_UTILITY_MODE = MODE_NO_ACT; 00565 TMR3_COUNT_TORQUE_NULL = 0; 00566 CUR_PRES_A_sum = 0; 00567 CUR_PRES_B_sum = 0; 00568 CUR_PRES_A_mean = 0; 00569 CUR_PRES_B_mean = 0; 00570 00571 ROM_RESET_DATA(); 00572 00573 dac_1 = PRES_A_VREF / 3.3f; 00574 dac_2 = PRES_B_VREF / 3.3f; 00575 //pc.printf("nulling end"); 00576 } 00577 } 00578 TMR3_COUNT_TORQUE_NULL++; 00579 break; 00580 } 00581 00582 // case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { 00583 // if (TMR3_COUNT_DEADZONE == 0) { 00584 // if (pos_plus_end == pos_minus_end) need_enc_init = true; 00585 // else temp_time = 0; 00586 // } 00587 // if (need_enc_init) { 00588 // if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) { 00589 // V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; 00590 // pos_plus_end = pos.sen; 00591 // } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) { 00592 // V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 00593 // pos_minus_end = pos.sen; 00594 // } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; 00595 // temp_time = TMR_FREQ_5k; 00596 // } 00597 // 00598 // if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) { 00599 // V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); 00600 // VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0; 00601 // 00602 // } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) { 00603 // V_out = 0; 00604 // CUR_VELOCITY_sum += CUR_VELOCITY; 00605 // } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) { 00606 // if (CUR_VELOCITY_sum == 0) DZ_dir = 1; 00607 // else if (CUR_VELOCITY_sum > 0) DZ_dir = 1; 00608 // else if (CUR_VELOCITY_sum < 0) DZ_dir = -1; 00609 // else DZ_temp_cnt2 = DZ_end; 00610 // CUR_VELOCITY_sum = 0; 00611 // } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) { 00612 // if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end; 00613 // 00614 // // Position of Dead Zone 00615 // // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0) 00616 // // | / | / |/ 00617 // // | ______/ ___|___/ ______/| 00618 // // |/ / | / | 00619 // // /| / | / | 00620 // // 0V 0V 0V 00621 // 00622 // if (DZ_temp_cnt2 < DZ_end) { 00623 // if (TMR3_COUNT_DEADZONE % 20 != 0) { 00624 // CUR_VELOCITY_sum += CUR_VELOCITY; 00625 // } else { 00626 // V_out -= DZ_dir; 00627 // if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++; 00628 // CUR_VELOCITY_sum = 0; 00629 // } 00630 // if (DZ_temp_cnt == 5) { 00631 // if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out; 00632 // else VALVE_DEADZONE_PLUS = (int16_t) V_out; 00633 // DZ_dir = -DZ_dir; 00634 // DZ_temp_cnt = 0; 00635 // DZ_temp_cnt2++; 00636 // } 00637 // } else { 00638 // TMR3_COUNT_DEADZONE = -1; 00639 // VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2; 00640 // if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) { 00641 // VALVE_DEADZONE_PLUS = VALVE_CENTER; 00642 // VALVE_DEADZONE_MINUS = VALVE_CENTER; 00643 // } 00644 // V_out = 0; 00645 // 00646 // ROM_RESET_DATA(); 00647 // 00648 // //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); 00649 // //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); 00650 // 00651 // CONTROL_MODE = MODE_NO_ACT; 00652 // DZ_temp_cnt2 = 0; 00653 // } 00654 // } 00655 // TMR3_COUNT_DEADZONE++; 00656 // break; 00657 // } 00658 00659 case MODE_FIND_HOME: { 00660 if (FINDHOME_STAGE == FINDHOME_INIT) { 00661 cnt_findhome = 0; 00662 cnt_vel_findhome = 0; 00663 //REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO 00664 pos.ref = pos.sen; 00665 vel.ref = 0.0f; 00666 I_ERR_INT = 0.0f; 00667 FINDHOME_STAGE = FINDHOME_GOTOLIMIT; 00668 } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) { 00669 int cnt_check_enc = (TMR_FREQ_5k/20); 00670 if(cnt_findhome%cnt_check_enc == 0) { 00671 FINDHOME_POSITION = pos.sen; 00672 FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD; 00673 FINDHOME_POSITION_OLD = FINDHOME_POSITION; 00674 } 00675 cnt_findhome++; 00676 00677 if (abs(FINDHOME_VELOCITY) <= 1) { 00678 cnt_vel_findhome = cnt_vel_findhome + 1; 00679 } else { 00680 cnt_vel_findhome = 0; 00681 } 00682 00683 if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec 00684 //REFERENCE_MODE = MODE_REF_NO_ACT; 00685 if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f; 00686 else pos.ref = pos.ref - 12.0f; 00687 00688 pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] 00689 00690 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { 00691 00692 double I_REF_POS = 0.0f; 00693 double temp_vel_pos = 0.0f; 00694 double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control 00695 00696 if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode 00697 temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // rad/s 00698 } else if ((OPERATING_MODE && 0x01) == 1) { 00699 temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // mm/s 00700 } 00701 if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f)))); 00702 else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); 00703 00704 I_REF = I_REF_POS; 00705 I_ERR_INT = 0.0f; 00706 00707 00708 } else { 00709 float VALVE_POS_RAW_FORCE_FB = 0.0f; 00710 VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); 00711 00712 if (VALVE_POS_RAW_FORCE_FB >= 0) { 00713 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS; 00714 } else { 00715 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS; 00716 } 00717 00718 VALVE_POS_CONTROL(valve_pos.ref); 00719 00720 V_out = (float) Vout.ref; 00721 00722 } 00723 00724 00725 00726 // pos.err = pos.ref_home_pos - pos.sen; 00727 // float VALVE_POS_RAW_POS_FB = 0.0f; 00728 // VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f; 00729 // valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; 00730 // VALVE_POS_CONTROL(valve_pos.ref); 00731 00732 cnt_findhome_wait = 0; 00733 // CONTROL_MODE = MODE_JOINT_CONTROL; 00734 alpha_trans = 0.0f; 00735 00736 00737 } else { 00738 ENC_SET(HOMEPOS_OFFSET); 00739 // ENC_SET_ZERO(); 00740 INIT_REF_POS = HOMEPOS_OFFSET; 00741 REF_POSITION = 0; 00742 REF_VELOCITY = 0; 00743 FINDHOME_POSITION = 0; 00744 FINDHOME_POSITION_OLD = 0; 00745 FINDHOME_VELOCITY = 0; 00746 00747 cnt_findhome = 0; 00748 cnt_vel_findhome = 0; 00749 FINDHOME_STAGE = FINDHOME_ZEROPOSE; 00750 00751 pos.ref = 0.0f; 00752 vel.ref = 0.0f; 00753 pos.ref_home_pos = 0.0f; 00754 vel.ref_home_pos = 0.0f; 00755 00756 I_ERR_INT = 0.0f; 00757 00758 // cnt_findhome_wait = 0; 00759 // FINDHOME_STAGE = FINDHOME_INIT; 00760 // CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL; 00761 } 00762 00763 // } else if (FINDHOME_STAGE == FINDHOME_WAIT) { 00764 // cnt_findhome_wait = cnt_findhome_wait + 1; 00765 // if(cnt_findhome_wait > 3*TMR_FREQ_5k){ 00766 // FINDHOME_STAGE = FINDHOME_ZEROPOSE; 00767 // pos.sen = 0.0f; 00768 // vel.sen = 0.0f; 00769 // } 00770 00771 } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) { 00772 int T_move = 2*TMR_FREQ_5k; 00773 pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS; 00774 vel.ref = 0.0f; 00775 00776 // input for position control 00777 00778 // CONTROL_MODE = MODE_JOINT_CONTROL; 00779 pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] 00780 00781 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { 00782 00783 double I_REF_POS = 0.0f; 00784 00785 double temp_vel_pos = 0.0f; 00786 double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control 00787 00788 if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode 00789 temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // rad/s 00790 } else if ((OPERATING_MODE && 0x01) == 1) { 00791 temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // mm/s 00792 } 00793 if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f)))); 00794 else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); 00795 00796 I_REF = I_REF_POS; 00797 I_ERR_INT = 0.0f; 00798 00799 00800 00801 } else { 00802 float VALVE_POS_RAW_FORCE_FB = 0.0f; 00803 VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); 00804 00805 if (VALVE_POS_RAW_FORCE_FB >= 0) { 00806 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS; 00807 } else { 00808 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS; 00809 } 00810 00811 VALVE_POS_CONTROL(valve_pos.ref); 00812 00813 V_out = (float) Vout.ref; 00814 00815 } 00816 00817 // pos.err = pos.ref - (float)pos.sen; 00818 // float VALVE_POS_RAW_POS_FB = 0.0f; 00819 // VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION; 00820 // valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; 00821 // VALVE_POS_CONTROL(valve_pos.ref); 00822 00823 cnt_findhome++; 00824 if (cnt_findhome >= T_move) { 00825 //REFERENCE_MODE = MODE_REF_DIRECT; 00826 cnt_findhome = 0; 00827 pos.ref = 0.0f; 00828 vel.ref = 0.0f; 00829 pos.ref_home_pos = 0.0f; 00830 vel.ref_home_pos = 0.0f; 00831 FINDHOME_STAGE = FINDHOME_INIT; 00832 CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL; 00833 } 00834 } 00835 00836 break; 00837 } 00838 00839 // case MODE_VALVE_GAIN_SETTING: { 00840 // if (TMR3_COUNT_FLOWRATE == 0) { 00841 // if (pos_plus_end == pos_minus_end) need_enc_init = true; 00842 // else { 00843 // V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 00844 // temp_time = (int) (0.5f * (float) TMR_FREQ_5k); 00845 // } 00846 // } 00847 // if (need_enc_init) { 00848 // if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) { 00849 // V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; 00850 // pos_plus_end = pos.sen; 00851 // } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) { 00852 // V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 00853 // pos_minus_end = pos.sen; 00854 // } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) { 00855 // need_enc_init = false; 00856 // check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end)); 00857 // check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end)); 00858 // check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init; 00859 // } 00860 // temp_time = TMR_FREQ_5k; 00861 // } 00862 // TMR3_COUNT_FLOWRATE++; 00863 // if (TMR3_COUNT_FLOWRATE > temp_time) { 00864 // if (flag_flowrate % 2 == 0) { // (+) 00865 // VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1); 00866 // V_out = VALVE_VOLTAGE; 00867 // if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) { 00868 // fl_temp_cnt++; 00869 // } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) { 00870 // VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000 00871 // // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec 00872 // fl_temp_cnt2++; 00873 // } 00874 // } else if (flag_flowrate % 2 == 1) { // (-) 00875 // VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1); 00876 // V_out = VALVE_VOLTAGE; 00877 // if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) { 00878 // fl_temp_cnt++; 00879 // } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) { 00880 // VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); 00881 // // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec 00882 // fl_temp_cnt2++; 00883 // } 00884 // } 00885 // if (fl_temp_cnt2 == 100) { 00886 // 00887 // ROM_RESET_DATA(); 00888 // 00889 // //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f)); 00890 // cur_vel_sum = 0; 00891 // fl_temp_cnt = 0; 00892 // fl_temp_cnt2 = 0; 00893 // flag_flowrate++; 00894 // } 00895 // if (flag_flowrate == 10) { 00896 // V_out = 0; 00897 // flag_flowrate = 0; 00898 // TMR3_COUNT_FLOWRATE = 0; 00899 // valve_gain_repeat_cnt++; 00900 // if (valve_gain_repeat_cnt >= 1) { 00901 // CONTROL_MODE = MODE_NO_ACT; 00902 // valve_gain_repeat_cnt = 0; 00903 // } 00904 // 00905 // } 00906 // break; 00907 // } 00908 // 00909 // } 00910 case MODE_PRESSURE_SENSOR_NULLING: { 00911 // DAC Voltage reference set 00912 if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) { 00913 CUR_PRES_A_sum += pres_A.sen; 00914 CUR_PRES_B_sum += pres_B.sen; 00915 00916 if (TMR3_COUNT_PRES_NULL % 10 == 0) { 00917 CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f; 00918 CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f; 00919 CUR_PRES_A_sum = 0; 00920 CUR_PRES_B_sum = 0; 00921 00922 float VREF_NullingGain = 0.0003f; 00923 PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean; 00924 PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean; 00925 00926 if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; 00927 if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; 00928 if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; 00929 if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; 00930 00931 dac_1 = PRES_A_VREF / 3.3f; 00932 dac_2 = PRES_B_VREF / 3.3f; 00933 } 00934 } else { 00935 CONTROL_UTILITY_MODE = MODE_NO_ACT; 00936 TMR3_COUNT_PRES_NULL = 0; 00937 CUR_PRES_A_sum = 0; 00938 CUR_PRES_B_sum = 0; 00939 CUR_PRES_A_mean = 0; 00940 CUR_PRES_B_mean = 0; 00941 00942 ROM_RESET_DATA(); 00943 00944 dac_1 = PRES_A_VREF / 3.3f; 00945 dac_2 = PRES_B_VREF / 3.3f; 00946 //pc.printf("nulling end"); 00947 } 00948 TMR3_COUNT_PRES_NULL++; 00949 break; 00950 } 00951 00952 // case MODE_PRESSURE_SENSOR_CALIB: { 00953 // if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) { 00954 // V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 00955 // if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) { 00956 // CUR_PRES_A_sum += CUR_PRES_A; 00957 // } 00958 // } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) { 00959 // V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; 00960 // if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) { 00961 // CUR_PRES_B_sum += CUR_PRES_B; 00962 // } 00963 // } else { 00964 // CONTROL_MODE = MODE_NO_ACT; 00965 // TMR3_COUNT_PRES_CALIB = 0; 00966 // V_out = 0; 00967 // PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL; 00968 // PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f); 00969 // PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL; 00970 // PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f); 00971 // CUR_PRES_A_sum = 0; 00972 // CUR_PRES_B_sum = 0; 00973 // CUR_PRES_A_mean = 0; 00974 // CUR_PRES_B_mean = 0; 00975 // 00976 // ROM_RESET_DATA(); 00977 // 00978 // //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f)); 00979 // //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f)); 00980 // } 00981 // TMR3_COUNT_PRES_CALIB++; 00982 // break; 00983 // } 00984 00985 // case MODE_ROTARY_FRICTION_TUNING: { 00986 // if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f); 00987 // V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f); 00988 // if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; 00989 // else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 00990 // TMR3_COUNT_ROTARY_FRIC_TUNE++; 00991 // if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) { 00992 // TMR3_COUNT_ROTARY_FRIC_TUNE = 0; 00993 // V_out = 0.0f; 00994 // CONTROL_MODE = MODE_NO_ACT; 00995 // } 00996 // break; 00997 // } 00998 00999 case MODE_DDV_POS_VS_PWM_ID: { 01000 CONTROL_MODE = MODE_VALVE_OPEN_LOOP; 01001 VALVE_ID_timer = VALVE_ID_timer + 1; 01002 01003 if(VALVE_ID_timer < TMR_FREQ_5k*1) { 01004 Vout.ref = 3000.0f * sin(2.0f*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); 01005 } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { 01006 Vout.ref = 1000.0f*(ID_index_array[ID_index]); 01007 } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { 01008 VALVE_POS_TMP = 0; 01009 data_num = 0; 01010 } else if(VALVE_ID_timer < TMR_FREQ_5k*3) { 01011 data_num = data_num + 1; 01012 VALVE_POS_TMP = VALVE_POS_TMP + value; 01013 } else if(VALVE_ID_timer == TMR_FREQ_5k*3) { 01014 Vout.ref = 0.0f; 01015 } else { 01016 VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num; 01017 VALVE_ID_timer = 0; 01018 ID_index= ID_index +1; 01019 } 01020 01021 if(ID_index>=25) { 01022 int i; 01023 VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; 01024 for(i=0; i<25; i++) { 01025 VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); 01026 if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) { 01027 VALVE_MAX_POS = VALVE_POS_AVG[i]; 01028 VALVE_POS_AVG_OLD = VALVE_MAX_POS; 01029 } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) { 01030 VALVE_MIN_POS = VALVE_POS_AVG[i]; 01031 VALVE_POS_AVG_OLD = VALVE_MIN_POS; 01032 } 01033 } 01034 ROM_RESET_DATA(); 01035 ID_index = 0; 01036 CONTROL_UTILITY_MODE = MODE_NO_ACT; 01037 } 01038 01039 01040 break; 01041 } 01042 01043 case MODE_DDV_DEADZONE_AND_CENTER: { 01044 CONTROL_MODE = MODE_VALVE_OPEN_LOOP; 01045 VALVE_DZ_timer = VALVE_DZ_timer + 1; 01046 if(first_check == 0) { 01047 if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { 01048 Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; 01049 } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01050 Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; 01051 pos_plus_end = pos.sen; 01052 } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { 01053 Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; 01054 } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { 01055 Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; 01056 pos_minus_end = pos.sen; 01057 } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) { 01058 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01059 } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) { 01060 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01061 data_num = data_num + 1; 01062 VALVE_POS_TMP = VALVE_POS_TMP + value; 01063 } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) { 01064 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01065 DDV_POS_AVG = VALVE_POS_TMP / data_num; 01066 START_POS = pos.sen; 01067 VALVE_POS_TMP = 0; 01068 data_num = 0; 01069 01070 } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) { 01071 valve_pos.ref = DDV_POS_AVG; 01072 VALVE_POS_CONTROL(valve_pos.ref); 01073 01074 } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) { 01075 valve_pos.ref = DDV_POS_AVG; 01076 VALVE_POS_CONTROL(valve_pos.ref); 01077 01078 } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) { 01079 valve_pos.ref = DDV_POS_AVG; 01080 VALVE_POS_CONTROL(valve_pos.ref); 01081 FINAL_POS = pos.sen; 01082 01083 if((FINAL_POS - START_POS)>200) { 01084 DZ_case = 1; 01085 } else if((FINAL_POS - START_POS)<-200) { 01086 DZ_case = -1; 01087 } else { 01088 DZ_case = 0; 01089 } 01090 01091 CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6)); 01092 01093 first_check = 1; 01094 DZ_DIRECTION = 1; 01095 VALVE_DZ_timer = 0; 01096 Ref_Valve_Pos_Old = DDV_POS_AVG; 01097 DZ_NUM = 1; 01098 DZ_index = 1; 01099 01100 } 01101 } else { 01102 if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) { 01103 if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) { 01104 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01105 //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; 01106 //CONTROL_MODE = MODE_JOINT_CONTROL; 01107 } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01108 START_POS = pos.sen; 01109 } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { 01110 valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; 01111 if(valve_pos.ref <= VALVE_MIN_POS) { 01112 valve_pos.ref = VALVE_MIN_POS; 01113 } else if(valve_pos.ref >= VALVE_MAX_POS) { 01114 valve_pos.ref = VALVE_MAX_POS; 01115 } 01116 VALVE_POS_CONTROL(valve_pos.ref); 01117 01118 } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { 01119 Ref_Valve_Pos_Old = valve_pos.ref; 01120 FINAL_POS = pos.sen; 01121 01122 if((FINAL_POS - START_POS)>100) { 01123 DZ_DIRECTION = 1 * DZ_case; 01124 } else if((FINAL_POS - START_POS)<-100) { 01125 DZ_DIRECTION = -1 * DZ_case; 01126 } else { 01127 DZ_DIRECTION = 1 * DZ_case; 01128 } 01129 01130 VALVE_DZ_timer = 0; 01131 DZ_index= DZ_index *2; 01132 if(DZ_index >= 128) { 01133 FIRST_DZ = valve_pos.ref; 01134 DZ_NUM = 2; 01135 Ref_Valve_Pos_Old = FIRST_DZ; 01136 DZ_index = 1; 01137 DZ_DIRECTION = 1; 01138 } 01139 } 01140 } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) { 01141 if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { 01142 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01143 //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; 01144 //CONTROL_MODE = MODE_JOINT_CONTROL; 01145 } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01146 START_POS = pos.sen; 01147 } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { 01148 valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; 01149 if(valve_pos.ref <= VALVE_MIN_POS) { 01150 valve_pos.ref = VALVE_MIN_POS; 01151 } else if(valve_pos.ref >= VALVE_MAX_POS) { 01152 valve_pos.ref = VALVE_MAX_POS; 01153 } 01154 VALVE_POS_CONTROL(valve_pos.ref); 01155 01156 } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { 01157 Vout.ref = 0.0f; 01158 } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { 01159 Ref_Valve_Pos_Old = valve_pos.ref; 01160 FINAL_POS = pos.sen; 01161 01162 if((FINAL_POS - START_POS)>100) { 01163 DZ_DIRECTION = 1 * DZ_case; 01164 } else if((FINAL_POS - START_POS)<-100) { 01165 DZ_DIRECTION = -1 * DZ_case; 01166 } else { 01167 DZ_DIRECTION = -1 * DZ_case; 01168 } 01169 01170 VALVE_DZ_timer = 0; 01171 DZ_index= DZ_index * 2; 01172 if(DZ_index >= 128) { 01173 SECOND_DZ = valve_pos.ref; 01174 VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); 01175 first_check = 0; 01176 VALVE_DEADZONE_MINUS = (float) FIRST_DZ; 01177 VALVE_DEADZONE_PLUS = (float) SECOND_DZ; 01178 01179 ROM_RESET_DATA(); 01180 01181 CONTROL_UTILITY_MODE = MODE_NO_ACT; 01182 DZ_index = 1; 01183 } 01184 } 01185 } else if(DZ_case == 0 && DZ_NUM ==1) { 01186 if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { 01187 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01188 //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; 01189 //CONTROL_MODE = MODE_JOINT_CONTROL; 01190 } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01191 START_POS = pos.sen; 01192 } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { 01193 valve_pos.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; 01194 if(valve_pos.ref <= VALVE_MIN_POS) { 01195 valve_pos.ref = VALVE_MIN_POS; 01196 } else if(valve_pos.ref >= VALVE_MAX_POS) { 01197 valve_pos.ref = VALVE_MAX_POS; 01198 } 01199 VALVE_POS_CONTROL(valve_pos.ref); 01200 01201 } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { 01202 Ref_Valve_Pos_Old = valve_pos.ref; 01203 FINAL_POS = pos.sen; 01204 01205 if((FINAL_POS - START_POS)>100) { 01206 DZ_DIRECTION = 1; 01207 } else if((FINAL_POS - START_POS)<-100) { 01208 DZ_DIRECTION = -1; 01209 } else { 01210 DZ_DIRECTION = 1; 01211 } 01212 VALVE_DZ_timer = 0; 01213 DZ_index= DZ_index *2; 01214 if(DZ_index >= 128) { 01215 FIRST_DZ = valve_pos.ref; 01216 DZ_NUM = 2; 01217 Ref_Valve_Pos_Old = FIRST_DZ; 01218 DZ_index = 1; 01219 DZ_DIRECTION = 1; 01220 } 01221 } 01222 } else { 01223 if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { 01224 Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01225 //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; 01226 //CONTROL_MODE = MODE_JOINT_CONTROL; 01227 } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01228 START_POS = pos.sen; 01229 } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { 01230 valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; 01231 if(valve_pos.ref <= VALVE_MIN_POS) { 01232 valve_pos.ref = VALVE_MIN_POS; 01233 } else if(valve_pos.ref > VALVE_MAX_POS) { 01234 valve_pos.ref = VALVE_MAX_POS - 1; 01235 } 01236 VALVE_POS_CONTROL(valve_pos.ref); 01237 01238 } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { 01239 Vout.ref = 0.0f; 01240 } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { 01241 Ref_Valve_Pos_Old = valve_pos.ref; 01242 FINAL_POS = pos.sen; 01243 01244 if((FINAL_POS - START_POS)>100) { 01245 DZ_DIRECTION = 1; 01246 } else if((FINAL_POS - START_POS)<-100) { 01247 DZ_DIRECTION = -1; 01248 } else { 01249 DZ_DIRECTION = 1; 01250 } 01251 01252 VALVE_DZ_timer = 0; 01253 DZ_index= DZ_index *2; 01254 if(DZ_index >= 128) { 01255 SECOND_DZ = valve_pos.ref; 01256 VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); 01257 first_check = 0; 01258 VALVE_DEADZONE_MINUS = (float) FIRST_DZ; 01259 VALVE_DEADZONE_PLUS = (float) SECOND_DZ; 01260 01261 ROM_RESET_DATA(); 01262 01263 CONTROL_UTILITY_MODE = MODE_NO_ACT; 01264 DZ_index = 1; 01265 } 01266 } 01267 } 01268 } 01269 break; 01270 } 01271 01272 case MODE_DDV_POS_VS_FLOWRATE: { 01273 CONTROL_MODE = MODE_VALVE_OPEN_LOOP; 01274 VALVE_FR_timer = VALVE_FR_timer + 1; 01275 if(first_check == 0) { 01276 if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { 01277 Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; 01278 //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); 01279 } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01280 Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; 01281 pos_plus_end = pos.sen; 01282 // CAN_TX_PRES((int16_t) (V_out), (int16_t) (7)); 01283 } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { 01284 Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; 01285 } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { 01286 // CAN_TX_PRES((int16_t) (V_out), (int16_t) (8)); 01287 Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; 01288 pos_minus_end = pos.sen; 01289 first_check = 1; 01290 VALVE_FR_timer = 0; 01291 valve_pos.ref = (float) VALVE_CENTER; 01292 ID_index = 0; 01293 max_check = 0; 01294 min_check = 0; 01295 } 01296 } else { 01297 if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { 01298 //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; 01299 pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end; 01300 CONTROL_MODE = MODE_JOINT_CONTROL; 01301 } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { 01302 data_num = 0; 01303 valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER; 01304 01305 VALVE_POS_CONTROL(valve_pos.ref); 01306 START_POS = pos.sen; 01307 } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) { 01308 valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER; 01309 VALVE_POS_CONTROL(valve_pos.ref); 01310 data_num = data_num + 1; 01311 if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) { 01312 FINAL_POS = pos.sen; 01313 one_period_end = 1; 01314 } 01315 } else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) { 01316 FINAL_POS = pos.sen; 01317 one_period_end = 1; 01318 V_out = 0.0f; 01319 } 01320 01321 if(one_period_end == 1) { 01322 if(valve_pos.ref > VALVE_MAX_POS) { 01323 max_check = 1; 01324 } else if(valve_pos.ref < VALVE_MIN_POS) { 01325 min_check = 1; 01326 } 01327 JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec 01328 01329 VALVE_FR_timer = 0; 01330 one_period_end = 0; 01331 ID_index= ID_index +1; 01332 V_out = 0.0f; 01333 } 01334 01335 if(max_check == 1 && min_check == 1) { 01336 01337 VALVE_POS_NUM = ID_index; 01338 ROM_RESET_DATA(); 01339 ID_index = 0; 01340 first_check = 0; 01341 VALVE_FR_timer = 0; 01342 CONTROL_UTILITY_MODE = MODE_NO_ACT; 01343 // CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); 01344 } 01345 } 01346 break; 01347 } 01348 01349 case MODE_SYSTEM_ID: { 01350 freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.; 01351 valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3); 01352 CONTROL_MODE = MODE_VALVE_OPEN_LOOP; 01353 cnt_sysid++; 01354 if (freq_sysid_Iref >= 300) { 01355 cnt_sysid = 0; 01356 CONTROL_UTILITY_MODE = MODE_NO_ACT; 01357 } 01358 break; 01359 } 01360 01361 01362 01363 default: 01364 break; 01365 } 01366 01367 // CONTROL MODE ------------------------------------------------------------ 01368 01369 switch (CONTROL_MODE) { 01370 case MODE_NO_ACT: { 01371 V_out = 0.0f; 01372 break; 01373 } 01374 01375 case MODE_VALVE_POSITION_CONTROL: { 01376 if (OPERATING_MODE == 5) { //SW Valve 01377 VALVE_POS_CONTROL(valve_pos.ref); 01378 V_out = Vout.ref; 01379 } else if (CURRENT_CONTROL_MODE == 0) { //PWM 01380 V_out = valve_pos.ref; 01381 } else { 01382 I_REF = valve_pos.ref * 0.001f; 01383 } 01384 01385 break; 01386 } 01387 01388 case MODE_JOINT_CONTROL: { 01389 double torq_ref = 0.0f; 01390 pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] 01391 vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] 01392 // vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] 01393 pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] 01394 01395 //K & D Low Pass Filter 01396 float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz 01397 K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D); 01398 D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D); 01399 01400 torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] 01401 01402 // torque feedback 01403 torq.err = torq_ref - torq.sen; //[N] 01404 torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] 01405 01406 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { 01407 01408 double I_REF_POS = 0.0f; 01409 double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback 01410 double I_REF_VC = 0.0f; // I_REF for velocity compensation 01411 01412 double temp_vel_pos = 0.0f; 01413 double temp_vel_torq = 0.0f; 01414 double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control 01415 01416 if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode 01417 temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s 01418 // L when P-gain = 100, f_cut = 10Hz L feedforward velocity 01419 } else if ((OPERATING_MODE && 0x01) == 1) { 01420 temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s 01421 // L when P-gain = 100, f_cut = 10Hz L feedforward velocity 01422 } 01423 if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f)))); 01424 else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); 01425 01426 // velocity compensation for torque control 01427 if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode 01428 I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); 01429 // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s 01430 temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s 01431 // L feedforward velocity 01432 } else if ((OPERATING_MODE && 0x01) == 1) { 01433 I_REF_FORCE_FB = 0.001f * 0.01f*((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive. 01434 // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s 01435 temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s 01436 // L feedforward velocity 01437 } 01438 if (temp_vel_torq > 0.0f) I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f)))); 01439 else I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); 01440 // L velocity(rad/s or mm/s) >> I_ref(mA) 01441 // Ref_Joint_FT_dot = (Ref_Joint_FT_Nm - Ref_Joint_FT_Nm_old) / TMR_DT_5k; 01442 // Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm; 01443 01444 I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB); 01445 01446 // Anti-windup for FT 01447 if (I_GAIN_JOINT_TORQUE != 0) { 01448 double I_MAX = 10.0f; // Maximum Current : 10mV 01449 double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f); 01450 if (I_REF > I_MAX) { 01451 double I_rem = I_REF - I_MAX; 01452 I_rem = Ka*I_rem; 01453 I_REF = I_MAX; 01454 torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k; 01455 } else if (I_REF < -I_MAX) { 01456 double I_rem = I_REF - (-I_MAX); 01457 I_rem = Ka*I_rem; 01458 I_REF = -I_MAX; 01459 torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k; 01460 } 01461 } 01462 01463 } else { 01464 float VALVE_POS_RAW_FORCE_FB = 0.0f; 01465 01466 VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f 01467 + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); 01468 01469 if (VALVE_POS_RAW_FORCE_FB >= 0) { 01470 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS; 01471 } else { 01472 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS; 01473 } 01474 01475 if(I_GAIN_JOINT_TORQUE != 0) { 01476 double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f; 01477 if(valve_pos.ref>VALVE_MAX_POS) { 01478 double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS; 01479 valve_pos_rem = valve_pos_rem * Ka; 01480 valve_pos.ref = VALVE_MAX_POS; 01481 torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; 01482 } else if(valve_pos.ref < VALVE_MIN_POS) { 01483 double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS; 01484 valve_pos_rem = valve_pos_rem * Ka; 01485 valve_pos.ref = VALVE_MIN_POS; 01486 torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; 01487 } 01488 } 01489 01490 VALVE_POS_CONTROL(valve_pos.ref); 01491 01492 // Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err); 01493 V_out = (float) Vout.ref; 01494 01495 } 01496 01497 break; 01498 } 01499 01500 case MODE_VALVE_OPEN_LOOP: { 01501 V_out = (float) Vout.ref; 01502 break; 01503 } 01504 01505 default: 01506 break; 01507 } 01508 01509 01510 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve 01511 01512 //////////////////////////////////////////////////////////////////////////// 01513 //////////////////////////// CURRENT CONTROL ////////////////////////////// 01514 //////////////////////////////////////////////////////////////////////////// 01515 01516 if (CURRENT_CONTROL_MODE) { 01517 01518 // Moog Valve Current Control Gain 01519 double R_model = 500.0f; // ohm 01520 double L_model = 1.2f; 01521 double w0 = 2.0f * 3.14f * 150.0f; 01522 double KP_I = 0.1f * L_model*w0; 01523 double KI_I = 0.1f * R_model*w0; 01524 01525 // KNR Valve Current Control Gain 01526 if (((OPERATING_MODE & 0b110)>>1) == 1) { // KNR Valve 01527 R_model = 163.0f; // ohm 01528 L_model = 1.0f; 01529 w0 = 2.0f * 3.14f * 80.0f; 01530 KP_I = 1.0f * L_model*w0; 01531 KI_I = 0.08f * R_model*w0; 01532 } 01533 01534 double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz 01535 I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; 01536 01537 FLAG_VALVE_DEADZONE = 1; 01538 01539 if (FLAG_VALVE_DEADZONE) { 01540 if (I_REF_fil > 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_PLUS / 1000.0f; // unit: mA 01541 else if (I_REF_fil < 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_MINUS / 1000.0f; // unit: mA 01542 else I_REF_fil_DZ = I_REF_fil + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f / 1000.0f; // unit: mA 01543 } else { 01544 I_REF_fil_DZ = I_REF_fil; 01545 } 01546 01547 I_ERR = I_REF_fil_DZ - cur.sen; 01548 I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; 01549 01550 01551 01552 01553 double FF_gain = 1.0f; 01554 01555 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT; 01556 // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV 01557 I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old; 01558 I_REF_fil_old = I_REF_fil_DZ; 01559 // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV 01560 VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV 01561 double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV 01562 01563 double Ka = 3.0f / KP_I; 01564 if (VALVE_PWM_RAW > V_MAX) { 01565 V_rem = VALVE_PWM_RAW - V_MAX; 01566 V_rem = Ka*V_rem; 01567 VALVE_PWM_RAW = V_MAX; 01568 I_ERR_INT = I_ERR_INT - V_rem * 0.0002f; 01569 } else if (VALVE_PWM_RAW < -V_MAX) { 01570 V_rem = VALVE_PWM_RAW - (-V_MAX); 01571 V_rem = Ka*V_rem; 01572 VALVE_PWM_RAW = -V_MAX; 01573 I_ERR_INT = I_ERR_INT - V_rem * 0.0002f; 01574 } 01575 Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse; 01576 } else { 01577 VALVE_PWM_RAW = I_REF * mV_PER_mA; 01578 Cur_Valve_Open_pulse = I_REF / mA_PER_pulse; 01579 } 01580 01581 //////////////////////////////////////////////////////////////////////////// 01582 ///////////////// Dead Zone Cancellation & Linearization ////////////////// 01583 //////////////////////////////////////////////////////////////////////////// 01584 // Dead Zone Cancellation (Mechanical Valve dead-zone) 01585 // if (FLAG_VALVE_DEADZONE) { 01586 // if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV 01587 // else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV 01588 // VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV 01589 // } else { 01590 // VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; 01591 // } 01592 01593 // VALVE_DEADZONE_PLUS : Current[mA] to start extraction 01594 // VALVE_DEADZONE_MINUS : Current[mA] to start contraction 01595 // double R_model = 500.0f; 01596 // FLAG_VALVE_DEADZONE = true; 01597 // if (FLAG_VALVE_DEADZONE) { 01598 // if (VALVE_PWM_RAW > 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * R_model / 1000.0f; // unit: mV 01599 // else if (VALVE_PWM_RAW < 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * R_model / 1000.0f; // unit: mV 01600 // else VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f* R_model / 1000.0f; 01601 // } else { 01602 VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; 01603 // } 01604 01605 // Output Voltage Linearization 01606 double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV 01607 double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000 01608 01609 // Dead Zone Cancellation (Electrical dead-zone) 01610 if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f); 01611 else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f); 01612 else V_out = (float) (CUR_PWM_lin); 01613 } 01614 01615 // if(V_out > 0.0f) V_out = (float) (V_out + 169.0f); 01616 // else if(V_out < 0.0f) V_out = (float) (V_out - 174.0f); 01617 // else V_out = V_out; 01618 01619 /******************************************************* 01620 *** PWM 01621 ********************************************************/ 01622 if(DIR_VALVE<0){ 01623 V_out = -V_out; 01624 } 01625 01626 if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) { 01627 V_out = VALVE_VOLTAGE_LIMIT*1000.0f; 01628 } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) { 01629 V_out = -VALVE_VOLTAGE_LIMIT*1000.0f; 01630 } 01631 PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Full duty : 12000.0mV 01632 01633 // Saturation of output voltage to 12.0V 01634 if(PWM_out > 1.0f) PWM_out=1.0f; 01635 else if (PWM_out < -1.0f) PWM_out=-1.0f; 01636 01637 if (PWM_out>0.0f) { 01638 dtc_v=0.0f; 01639 dtc_w=PWM_out; 01640 } else { 01641 dtc_v=-PWM_out; 01642 dtc_w=0.0f; 01643 } 01644 01645 //pwm 01646 TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); 01647 TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); 01648 01649 01650 if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { 01651 01652 // Position, Velocity, and Torque (ID:1200) 01653 if (flag_data_request[0] == HIGH) { 01654 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator 01655 //if (SENSING_MODE == 0) { 01656 CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); 01657 //} else if (SENSING_MODE == 1) { 01658 // CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); 01659 //} 01660 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator 01661 //if (SENSING_MODE == 0) { 01662 CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); 01663 //} else if (SENSING_MODE == 1) { 01664 // CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); 01665 //} 01666 } 01667 } 01668 if (flag_data_request[1] == HIGH) { 01669 //valve position 01670 double t_value = 0; 01671 // if(valve_pos.ref>=(float) VALVE_CENTER) { 01672 // t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); 01673 // } else { 01674 // t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); 01675 // } 01676 if(OPERATING_MODE==5) { 01677 t_value = (double) value; 01678 } else if(CURRENT_CONTROL_MODE==1) { 01679 t_value = cur.sen; 01680 } else { 01681 t_value = V_out; 01682 } 01683 // CAN_TX_TORQUE((int16_t) (I_REF_fil_DZ * 1000.0f)); //1300 01684 CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300 01685 //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300 01686 } 01687 01688 01689 if (flag_data_request[2] == HIGH) { 01690 //pressure A and B 01691 CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400 01692 } 01693 01694 // If it doesn't rest, below can can not work. 01695 for (can_rest = 0; can_rest < 10000; can_rest++) { 01696 ; 01697 } 01698 01699 if (flag_data_request[3] == HIGH) { 01700 //PWM 01701 CAN_TX_PWM((int16_t) value); //1500 01702 } 01703 //for (i = 0; i < 10000; i++) { 01704 // ; 01705 // } 01706 if (flag_data_request[4] == HIGH) { 01707 //valve position 01708 CAN_TX_VALVE_POSITION((int16_t) (CAN_FREQ), (int16_t) (D_DAMPER), (int16_t) OPERATING_MODE); //1600 01709 } 01710 01711 // Others : Reference position, Reference FT, PWM, Current (ID:1300) 01712 // if (flag_data_request[1] == HIGH) { 01713 // CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3)); 01714 // } 01715 //if (flag_delay_test == 1){ 01716 //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref); 01717 //} 01718 01719 TMR2_COUNT_CAN_TX = 0; 01720 } 01721 TMR2_COUNT_CAN_TX++; 01722 01723 } 01724 TIM3->SR = 0x0; // reset the status register 01725 01726 } 01727 01728 void SystemClock_Config(void) 01729 { 01730 RCC_OscInitTypeDef RCC_OscInitStruct = {0}; 01731 RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; 01732 01733 /** Configure the main internal regulator output voltage 01734 */ 01735 __HAL_RCC_PWR_CLK_ENABLE(); 01736 __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); 01737 /** Initializes the CPU, AHB and APB busses clocks 01738 */ 01739 RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; 01740 RCC_OscInitStruct.HSIState = RCC_HSI_ON; 01741 RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; 01742 RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; 01743 RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; 01744 RCC_OscInitStruct.PLL.PLLM = 8; //8 01745 RCC_OscInitStruct.PLL.PLLN = 180;//180 01746 RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; 01747 RCC_OscInitStruct.PLL.PLLQ = 2; 01748 RCC_OscInitStruct.PLL.PLLR = 2; 01749 if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { 01750 //Error_Handler(); 01751 } 01752 /** Activate the Over-Drive mode 01753 */ 01754 if (HAL_PWREx_EnableOverDrive() != HAL_OK) { 01755 //Error_Handler(); 01756 } 01757 /** Initializes the CPU, AHB and APB busses clocks 01758 */ 01759 RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK 01760 |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; 01761 RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; 01762 RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; 01763 RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; 01764 RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; 01765 01766 if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { 01767 //Error_Handler(); 01768 } 01769 HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1); 01770 HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_SYSCLK, RCC_MCODIV_2); 01771 }
Generated on Mon Jul 20 2020 06:59:20 by
1.7.2