Buyoun Cho
/
HydraulicControlBoard_PostLIGHT_210420
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //Hydraulic Control Board 00002 //distributed by Sungwoo Kim 00003 // 2020/12/28 00004 //revised by Buyoun Cho 00005 // 2021/04/20 00006 00007 // 유의사항 00008 // 소수 적을때 뒤에 f 꼭 붙이기 00009 // CAN 선은 ground까지 있는 3상 선으로 써야함. 00010 // 전원은 12~24V 인가. 00011 00012 #include "mbed.h" 00013 #include "FastPWM.h" 00014 #include "INIT_HW.h" 00015 #include "function_CAN.h" 00016 #include "SPI_EEP_ENC.h" 00017 #include "I2C_AS5510.h" 00018 #include "setting.h" 00019 #include "function_utilities.h" 00020 #include "stm32f4xx_flash.h" 00021 #include "FlashWriter.h" 00022 #include <string> 00023 #include <iostream> 00024 #include <cmath> 00025 00026 using namespace std; 00027 Timer t; 00028 00029 // dac & check /////////////////////////////////////////// 00030 DigitalOut check(PC_2); 00031 DigitalOut check_2(PC_3); 00032 AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f 00033 AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f 00034 AnalogIn adc1(PC_4); //pressure_1 00035 AnalogIn adc2(PB_0); //pressure_2 00036 AnalogIn adc3(PC_1); //current 00037 00038 // PWM /////////////////////////////////////////// 00039 float dtc_v=0.0f; 00040 float dtc_w=0.0f; 00041 00042 // I2C /////////////////////////////////////////// 00043 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F) 00044 const int i2c_slave_addr1 = 0x56; // AS5510 address 00045 unsigned int value; // 10bit output of reading sensor AS5510 00046 00047 // SPI /////////////////////////////////////////// 00048 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK); 00049 DigitalOut eeprom_cs(PB_12); 00050 SPI enc(PC_12,PC_11,PC_10); 00051 DigitalOut enc_cs(PD_2); 00052 DigitalOut LED(PA_15); 00053 00054 // UART /////////////////////////////////////////// 00055 Serial pc(PA_9,PA_10); // _ UART 00056 00057 // CAN /////////////////////////////////////////// 00058 CAN can(PB_8, PB_9, 1000000); 00059 CANMessage msg; 00060 void onMsgReceived() 00061 { 00062 CAN_RX_HANDLER(); 00063 } 00064 00065 // Variables /////////////////////////////////////////// 00066 State pos; 00067 State vel; 00068 State Vout; 00069 State force; 00070 State torq; // unit : N 00071 State torq_dot; 00072 State pres_A; // unit : bar 00073 State pres_B; 00074 State cur; // unit : mA 00075 State valve_pos; 00076 00077 State INIT_Vout; 00078 State INIT_Valve_Pos; 00079 State INIT_Pos; 00080 State INIT_torq; 00081 00082 extern int CID_RX_CMD; 00083 extern int CID_RX_REF_POSITION; 00084 extern int CID_RX_REF_OPENLOOP; 00085 extern int CID_RX_REF_PWM; 00086 00087 extern int CID_TX_INFO; 00088 extern int CID_TX_POS_VEL_TORQ; 00089 extern int CID_TX_PWM; 00090 extern int CID_TX_CURRENT; 00091 extern int CID_TX_VOUT; 00092 extern int CID_TX_VALVE_POSITION; 00093 extern int CID_TX_SOMETHING; 00094 00095 float temp_P_GAIN = 0.0f; 00096 float temp_I_GAIN = 0.0f; 00097 int temp_VELOCITY_COMP_GAIN = 0; 00098 00099 inline float tanh_inv(float y) { 00100 if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f; 00101 if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f; 00102 return log(sqrt((1.0f+y)/(1.0f-y))); 00103 } 00104 00105 00106 /******************************************************************************* 00107 * REFERENCE MODE 00108 ******************************************************************************/ 00109 enum _REFERENCE_MODE { 00110 MODE_REF_NO_ACT = 0, 00111 MODE_REF_DIRECT, 00112 MODE_REF_FINDHOME 00113 }; 00114 00115 /******************************************************************************* 00116 * CONTROL MODE 00117 ******************************************************************************/ 00118 enum _CONTROL_MODE { 00119 //control mode 00120 MODE_NO_ACT = 0, //0 00121 MODE_VALVE_POSITION_CONTROL, //1 00122 MODE_JOINT_CONTROL, //2 00123 00124 MODE_VALVE_OPEN_LOOP, //3 00125 MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4 00126 MODE_RL, //5 00127 00128 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6 00129 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7 00130 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8 00131 00132 MODE_TEST_CURRENT_CONTROL, //9 00133 MODE_TEST_PWM_CONTROL, //10 00134 00135 MODE_CURRENT_CONTROL, //11 00136 MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12 00137 MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13 00138 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //14 00139 00140 //utility 00141 MODE_TORQUE_SENSOR_NULLING = 20, //20 00142 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21 00143 MODE_FIND_HOME, //22 00144 MODE_VALVE_GAIN_SETTING, //23 00145 MODE_PRESSURE_SENSOR_NULLING, //24 00146 MODE_PRESSURE_SENSOR_CALIB, //25 00147 MODE_ROTARY_FRICTION_TUNING, //26 00148 00149 MODE_DDV_POS_VS_PWM_ID = 30, //30 00150 MODE_DDV_DEADZONE_AND_CENTER, //31 00151 MODE_DDV_POS_VS_FLOWRATE, //32 00152 MODE_SYSTEM_ID, //33 00153 MODE_FREQ_TEST, //34 00154 MODE_SEND_BUFFER, //35 00155 MODE_SEND_OVER, //36 00156 MODE_STEP_TEST, //37 00157 }; 00158 00159 void SystemClock_Config(void) 00160 { 00161 RCC_OscInitTypeDef RCC_OscInitStruct = {0}; 00162 RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; 00163 00164 /* Configure the main internal regulator output voltage 00165 */ 00166 __HAL_RCC_PWR_CLK_ENABLE(); 00167 __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); 00168 /* Initializes the CPU, AHB and APB busses clocks 00169 */ 00170 RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; 00171 RCC_OscInitStruct.HSIState = RCC_HSI_ON; 00172 RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; 00173 RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; 00174 RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; 00175 RCC_OscInitStruct.PLL.PLLM = 8;//8 00176 RCC_OscInitStruct.PLL.PLLN = 180; //180 00177 RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; 00178 RCC_OscInitStruct.PLL.PLLQ = 2; 00179 RCC_OscInitStruct.PLL.PLLR = 2; 00180 if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { 00181 //Error_Handler(); 00182 } 00183 /** Activate the Over-Drive mode 00184 */ 00185 if (HAL_PWREx_EnableOverDrive() != HAL_OK) { 00186 //Error_Handler(); 00187 } 00188 /** Initializes the CPU, AHB and APB busses clocks 00189 */ 00190 RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK 00191 |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; 00192 RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; 00193 RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; 00194 RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; 00195 RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; 00196 00197 if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { 00198 //Error_Handler(); 00199 } 00200 } 00201 00202 00203 int main() 00204 { 00205 /********************************* 00206 *** Initialization 00207 *********************************/ 00208 00209 HAL_Init(); 00210 SystemClock_Config(); 00211 00212 LED = 0; 00213 pc.baud(9600); 00214 00215 // i2c init 00216 // i2c.frequency(400 * 1000); // 0.4 mHz 00217 // wait_ms(2); // Power Up wait 00218 // look_for_hardware_i2c(); // Hardware present 00219 // init_as5510(i2c_slave_addr1); 00220 // make_delay(); 00221 00222 // spi init 00223 eeprom_cs = 1; 00224 eeprom.format(8,3); 00225 eeprom.frequency(5000000); //5M 00226 eeprom_cs = 0; 00227 make_delay(); 00228 00229 enc_cs = 1; //sw add 00230 enc.format(8,0); 00231 enc.frequency(5000000); //10M 00232 enc_cs = 0; //sw add 00233 00234 make_delay(); 00235 00236 // spi _ enc 00237 spi_enc_set_init(); 00238 make_delay(); 00239 00240 ////// bno rom 00241 // spi_eeprom_write(RID_BNO, (int16_t)1); 00242 // make_delay(); 00243 //////// 00244 00245 // rom 00246 ROM_CALL_DATA(); 00247 make_delay(); 00248 00249 // ADC init 00250 Init_ADC(); 00251 make_delay(); 00252 00253 // Pwm init 00254 Init_PWM(); 00255 TIM4->CR1 ^= TIM_CR1_UDIS; 00256 make_delay(); 00257 00258 // CAN 00259 can.attach(&CAN_RX_HANDLER); 00260 CAN_ID_INIT(); 00261 make_delay(); 00262 00263 //can.reset(); 00264 can.filter(msg.id, 0xFFFFF000, CANStandard); 00265 00266 // TMR3 init 00267 Init_TMR3(); 00268 TIM3->CR1 ^= TIM_CR1_UDIS; 00269 make_delay(); 00270 00271 //Timer priority 00272 NVIC_SetPriority(TIM3_IRQn, 2); 00273 NVIC_SetPriority(TIM4_IRQn, 3); 00274 00275 00276 //DAC init 00277 if (SENSING_MODE == 0) { 00278 dac_1 = FORCE_VREF / 3.3f; 00279 dac_2 = 0.0f; 00280 } else if (SENSING_MODE == 1) { 00281 if (DIR_VALVE_ENC > 0) { 00282 dac_1 = PRES_A_VREF / 3.3f; 00283 dac_2 = PRES_B_VREF / 3.3f; 00284 } else { 00285 dac_1 = PRES_B_VREF / 3.3f; 00286 dac_2 = PRES_A_VREF / 3.3f; 00287 } 00288 } 00289 make_delay(); 00290 00291 for (int i=0; i<50; i++) { 00292 if(i%2==0) 00293 ID_index_array[i] = - i * 0.5f; 00294 else 00295 ID_index_array[i] = (i+1) * 0.5f; 00296 } 00297 00298 /************************************ 00299 *** Program is operating! 00300 *************************************/ 00301 while(1) { 00302 00303 // UART example 00304 // if(timer_while==100000) { 00305 // timer_while = 0; 00306 // pc.printf("%f\n", value); 00307 // } 00308 // timer_while ++; 00309 00310 //i2c for SW valve 00311 //if(OPERATING_MODE == 5) { 00312 // read_field(i2c_slave_addr1); 00313 // if(DIR_VALVE_ENC < 0) value = 1023 - value; 00314 // } 00315 } 00316 } 00317 00318 00319 // Velocity feedforward for SW valve 00320 float DDV_JOINT_POS_FF(float REF_JOINT_VEL) 00321 { 00322 int i = 0; 00323 float Ref_Valve_Pos_FF = 0.0f; 00324 for(i=0; i<VALVE_POS_NUM; i++) { 00325 if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) { 00326 if(i==0) { 00327 if(JOINT_VEL[i+1] == JOINT_VEL[i]) { 00328 Ref_Valve_Pos_FF = (float) VALVE_CENTER; 00329 } else { 00330 Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER; 00331 } 00332 } else { 00333 if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) { 00334 Ref_Valve_Pos_FF = (float) VALVE_CENTER; 00335 } else { 00336 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]); 00337 } 00338 } 00339 break; 00340 } 00341 } 00342 if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { 00343 Ref_Valve_Pos_FF = (float) VALVE_MAX_POS; 00344 } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { 00345 Ref_Valve_Pos_FF = (float) VALVE_MIN_POS; 00346 } 00347 00348 Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER); //VELOCITY_COMP_GAIN : 0~100 00349 return Ref_Valve_Pos_FF; 00350 } 00351 00352 // Valve feedforward for SW valve 00353 void VALVE_POS_CONTROL(float REF_VALVE_POS) 00354 { 00355 int i = 0; 00356 00357 if(REF_VALVE_POS > VALVE_MAX_POS) { 00358 REF_VALVE_POS = VALVE_MAX_POS; 00359 } else if(REF_VALVE_POS < VALVE_MIN_POS) { 00360 REF_VALVE_POS = VALVE_MIN_POS; 00361 } 00362 valve_pos_err = (float) (REF_VALVE_POS - value); 00363 valve_pos_err_diff = valve_pos_err - valve_pos_err_old; 00364 valve_pos_err_old = valve_pos_err; 00365 valve_pos_err_sum += valve_pos_err; 00366 if (valve_pos_err_sum > 1000.0f) valve_pos_err_sum = 1000.0f; 00367 if (valve_pos_err_sum<-1000.0f) valve_pos_err_sum = -1000.0f; 00368 00369 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; 00370 00371 for(i=0; i<24; i++) { 00372 if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { 00373 if(i==0) { 00374 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]); 00375 } else { 00376 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]; 00377 } 00378 break; 00379 } 00380 } 00381 Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; 00382 } 00383 00384 // PWM duty vs. voltage output of L6205 in STM board 00385 #define LT_MAX_IDX 57 00386 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, 00387 -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f, 00388 -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f, 00389 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f, 00390 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f, 00391 25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f 00392 }; // duty 00393 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, 00394 -150.0f, -145.0f, -145.0f, -145.0f, -135.0f, -135.0f, -135.0f, -127.5f, -127.5f, -115.0f, 00395 -115.0f, -115.0F, -100.0f, -100.0f, -100.0f, -60.0f, -60.0f, -10.0f, -5.0f, 0.0f, 00396 7.5f, 14.0f, 14.0f, 14.0f, 42.5f, 42.5f, 42.5f, 80.0f, 80.0f, 105.0f, 00397 105.0f, 105.0f, 120.0f, 120.0f, 120.0f, 131.0f, 131.0f, 140.0f, 140.0f, 140.0f, 00398 155.0f, 160.0f, 170.0f, 174.0f, 182.0f, 191.0f, 212.0f, 230.0f 00399 }; // mV 00400 00401 float PWM_duty_byLT(float Ref_V) 00402 { 00403 float PWM_duty = 0.0f; 00404 if(Ref_V<LT_Voltage_Output[0]) { 00405 PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5f+LT_PWM_duty[0]; 00406 } else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) { 00407 PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5f+LT_PWM_duty[LT_MAX_IDX-1]; 00408 } else { 00409 int idx = 0; 00410 for(idx=0; idx<LT_MAX_IDX-1; idx++) { 00411 float ini_x = LT_Voltage_Output[idx]; 00412 float fin_x = LT_Voltage_Output[idx+1]; 00413 float ini_y = LT_PWM_duty[idx]; 00414 float fin_y = LT_PWM_duty[idx+1]; 00415 if(Ref_V>=ini_x && Ref_V<fin_x) { 00416 PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y; 00417 break; 00418 } 00419 } 00420 } 00421 00422 return PWM_duty; 00423 } 00424 00425 00426 00427 /******************************************************************************* 00428 TIMER INTERRUPT 00429 *******************************************************************************/ 00430 00431 //------------------------------------------------ 00432 // TMR4 : Sensor Read & Data Handling 00433 //----------------------------------------------- 00434 float FREQ_TMR4 = (float)FREQ_20k; 00435 float DT_TMR4 = (float)DT_20k; 00436 long CNT_TMR4 = 0; 00437 int TMR4_FREQ_10k = (int)FREQ_10k; 00438 extern "C" void TIM4_IRQHandler(void) 00439 { 00440 if (TIM4->SR & TIM_SR_UIF ) { 00441 00442 // Current =================================================== 00443 //ADC3->CR2 |= 0x40000000; // adc _ 12bit 00444 00445 cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA 00446 00447 // Encoder =================================================== 00448 if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) { 00449 ENC_UPDATE(); 00450 } 00451 00452 // Force or Pressure Transducer ============================================= 00453 ADC1->CR2 |= 0x40000000; 00454 if (SENSING_MODE == 0) { // Force sensing 00455 force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N 00456 } else if (SENSING_MODE == 1) { // Pressure sensing 00457 float pres_A_new, pres_B_new; 00458 if (DIR_VALVE_ENC > 0) { 00459 pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar 00460 pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; 00461 } else { 00462 pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar 00463 pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; 00464 } 00465 pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f); 00466 pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f); 00467 00468 if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator 00469 float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm 00470 torq.UpdateSen(torq_new,FREQ_TMR4,1000.0); // unit : Nm 00471 } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator 00472 float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N 00473 force.UpdateSen(force_new,FREQ_TMR4,1000.0); // unit : N 00474 } 00475 } 00476 00477 CNT_TMR4++; 00478 } 00479 TIM4->SR = 0x0; // reset the status register 00480 } 00481 00482 00483 int j =0; 00484 float FREQ_TMR3 = (float)FREQ_5k; 00485 float DT_TMR3 = (float)DT_5k; 00486 int cnt_trans = 0; 00487 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f; 00488 int can_rest =0; 00489 00490 extern "C" void TIM3_IRQHandler(void) 00491 { 00492 if (TIM3->SR & TIM_SR_UIF ) { 00493 00494 if(MODE_POS_FT_TRANS == 1) { 00495 if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; 00496 alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; 00497 cnt_trans++; 00498 torq.err_sum = 0; 00499 if((float)cnt_trans * DT_TMR3 > 3.0f) 00500 MODE_POS_FT_TRANS = 2; 00501 } else if(MODE_POS_FT_TRANS == 3) { 00502 if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0; 00503 alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; 00504 cnt_trans++; 00505 torq.err_sum = 0; 00506 if((float) cnt_trans * DT_TMR3 > 3.0f ) 00507 MODE_POS_FT_TRANS = 0; 00508 } else if(MODE_POS_FT_TRANS == 2) { 00509 alpha_trans = 1.0f; 00510 cnt_trans = 0; 00511 } else { 00512 alpha_trans = 0.0f; 00513 cnt_trans = 0; 00514 } 00515 00516 00517 // Reference Update ========================================================== 00518 switch (REFERENCE_MODE) { 00519 case MODE_REF_NO_ACT: { 00520 break; 00521 } 00522 case MODE_REF_DIRECT: { 00523 pos.ref = REF_POSITION; 00524 vel.ref = REF_VELOCITY; 00525 torq.ref = REF_TORQUE; 00526 force.ref = REF_FORCE; 00527 break; 00528 } 00529 case MODE_REF_FINDHOME: { 00530 pos.ref = REF_POSITION_FINDHOME; 00531 vel.ref = 0.0f; 00532 torq.ref = 0.0f; 00533 force.ref = 0.0f; 00534 break; 00535 } 00536 default: 00537 break; 00538 } 00539 00540 if (((OPERATING_MODE&0b010)>>1) == 0) { 00541 K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv); 00542 K_v = 0.16f; 00543 mV_PER_mA = 500.0f; // 5000mV/10mA 00544 mV_PER_pulse = 0.5f; // 5000mV/10000pulse 00545 mA_PER_pulse = 0.001f; // 10mA/10000pulse 00546 } else if (((OPERATING_MODE&0b010)>>1) == 1) { 00547 K_v = 0.5f; // KNR (LPM >> mA) , 100bar 00548 mV_PER_mA = 166.6666f; // 5000mV/30mA 00549 mV_PER_pulse = 0.5f; // 5000mV/10000pulse 00550 mA_PER_pulse = 0.003f; // 30mA/10000pulse 00551 } 00552 00553 // ===================================================================== 00554 // CONTROL LOOP -------------------------------------------------------- 00555 // ===================================================================== 00556 int UTILITY_MODE = 0; 00557 int CONTROL_MODE = 0; 00558 00559 if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) { 00560 UTILITY_MODE = CONTROL_UTILITY_MODE; 00561 CONTROL_MODE = MODE_NO_ACT; 00562 } else { 00563 CONTROL_MODE = CONTROL_UTILITY_MODE; 00564 UTILITY_MODE = MODE_NO_ACT; 00565 } 00566 // UTILITY MODE ------------------------------------------------------------ 00567 switch (UTILITY_MODE) { 00568 case MODE_NO_ACT: { 00569 break; 00570 } 00571 00572 case MODE_TORQUE_SENSOR_NULLING: 00573 { 00574 static float FORCE_pulse_sum = 0.0; 00575 static float PresA_pulse_sum = 0.0; 00576 static float PresB_pulse_sum = 0.0; 00577 00578 // DAC Voltage reference set 00579 float VREF_TuningGain = 0.000003f; 00580 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) { 00581 LED = 1; 00582 if(SENSING_MODE == 0) { // Force Sensor (Loadcell) 00583 FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE; 00584 if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { 00585 float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f; 00586 FORCE_pulse_sum = 0.0f; 00587 00588 FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean); 00589 if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f; 00590 if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f; 00591 dac_1 = FORCE_VREF / 3.3f; 00592 } 00593 } else if (SENSING_MODE == 1) { // Pressure Sensor 00594 PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR; 00595 PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR; 00596 if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { 00597 float PresA_pluse_mean = PresA_pulse_sum / 10.0f; 00598 float PresB_pluse_mean = PresB_pulse_sum / 10.0f; 00599 PresA_pulse_sum = 0.0f; 00600 PresB_pulse_sum = 0.0f; 00601 00602 PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean); 00603 if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; 00604 if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; 00605 dac_1 = PRES_A_VREF / 3.3f; 00606 PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean); 00607 if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; 00608 if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; 00609 dac_2 = PRES_B_VREF / 3.3f; 00610 } 00611 } 00612 TMR3_COUNT_TORQUE_NULL++; 00613 } else { 00614 if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell) 00615 FORCE_pulse_sum = 0.0f; 00616 dac_1 = FORCE_VREF / 3.3f; 00617 spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f)); 00618 } else if (SENSING_MODE == 1) { 00619 PresA_pulse_sum = 0.0f; 00620 PresB_pulse_sum = 0.0f; 00621 dac_1 = PRES_A_VREF / 3.3f; 00622 dac_2 = PRES_B_VREF / 3.3f; 00623 spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f)); 00624 spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f)); 00625 } 00626 CONTROL_UTILITY_MODE = MODE_NO_ACT; 00627 TMR3_COUNT_TORQUE_NULL = 0; 00628 } 00629 break; 00630 } 00631 00632 case MODE_FIND_HOME: 00633 { 00634 static int cnt_findhome = 0; 00635 static int cnt_terminate_findhome = 0; 00636 static float FINDHOME_POSITION_pulse = 0.0f; 00637 static float FINDHOME_POSITION_pulse_OLD = 0.0f; 00638 static float FINDHOME_VELOCITY_pulse = 0.0f; 00639 static float REF_POSITION_FINDHOME_INIT = 0.0f; 00640 00641 if (FINDHOME_STAGE == FINDHOME_INIT) { 00642 REFERENCE_MODE = MODE_REF_FINDHOME; 00643 cnt_findhome = 0; 00644 cnt_terminate_findhome = 0; 00645 pos.ref = pos.sen; 00646 vel.ref = 0.0f; 00647 REF_POSITION_FINDHOME = pos.ref; 00648 FINDHOME_STAGE = FINDHOME_GOTOLIMIT; 00649 } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) { 00650 int cnt_check_enc = (TMR_FREQ_5k/20); // 5000/20 = 250tic = 50msec 00651 if(cnt_findhome%cnt_check_enc == 0) { 00652 FINDHOME_POSITION_pulse = pos.sen*ENC_PULSE_PER_POSITION; 00653 FINDHOME_VELOCITY_pulse = FINDHOME_POSITION_pulse - FINDHOME_POSITION_pulse_OLD; 00654 FINDHOME_POSITION_pulse_OLD = FINDHOME_POSITION_pulse; 00655 } 00656 cnt_findhome++; 00657 00658 if (fabs(FINDHOME_VELOCITY_pulse) <= 1) { 00659 cnt_terminate_findhome = cnt_terminate_findhome + 1; 00660 } else { 00661 cnt_terminate_findhome = 0; 00662 } 00663 00664 if ((cnt_terminate_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec 00665 double GOTOHOME_SPEED = 10.0f; // 20mm/s or 20deg/s 00666 if (HOMEPOS_OFFSET > 0) { 00667 REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k; 00668 } else { 00669 REF_POSITION_FINDHOME = REF_POSITION_FINDHOME - GOTOHOME_SPEED*DT_5k; 00670 } 00671 CONTROL_MODE = MODE_JOINT_CONTROL; 00672 alpha_trans = 0.0f; 00673 } else { 00674 ENC_SET((long)((long)HOMEPOS_OFFSET*10)); 00675 REF_POSITION_FINDHOME_INIT = (float)((long)HOMEPOS_OFFSET*10); 00676 FINDHOME_POSITION_pulse = 0; 00677 FINDHOME_POSITION_pulse_OLD = 0; 00678 FINDHOME_VELOCITY_pulse = 0; 00679 00680 cnt_findhome = 0; 00681 cnt_terminate_findhome = 0; 00682 pos.ref = 0.0f; 00683 FINDHOME_STAGE = FINDHOME_ZEROPOSE; 00684 } 00685 } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) { 00686 00687 int T_move = 2*TMR_FREQ_5k; 00688 REF_POSITION_FINDHOME = (0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT; 00689 00690 cnt_findhome++; 00691 if (cnt_findhome >= T_move) { 00692 cnt_findhome = 0; 00693 pos.ref = 0.0f; 00694 FINDHOME_STAGE = FINDHOME_INIT; 00695 CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL; 00696 REFERENCE_MODE = MODE_REF_DIRECT; 00697 } 00698 } 00699 break; 00700 } 00701 default: 00702 break; 00703 } 00704 00705 // CONTROL MODE ------------------------------------------------------------ 00706 switch (CONTROL_MODE) { 00707 case MODE_NO_ACT: { 00708 V_out = 0.0f; 00709 break; 00710 } 00711 00712 case MODE_VALVE_POSITION_CONTROL: { 00713 if (OPERATING_MODE == 5) { //SW Valve 00714 VALVE_POS_CONTROL(valve_pos.ref); 00715 V_out = Vout.ref; 00716 } else if (CURRENT_CONTROL_MODE == 0) { //PWM 00717 V_out = valve_pos.ref; 00718 } else { 00719 I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA 00720 float I_MAX = 10.0f; // Max : 10mA 00721 if (I_REF > I_MAX) { 00722 I_REF = I_MAX; 00723 } else if (I_REF < -I_MAX) { 00724 I_REF = -I_MAX; 00725 } 00726 } 00727 break; 00728 } 00729 00730 case MODE_JOINT_CONTROL: 00731 { 00732 00733 float temp_vel_pos = 0.0f; // desired velocity for position control 00734 float temp_vel_FT = 0.0f; // desired velocity for force/torque control 00735 float temp_vel_ff = 0.0f; // desired velocity for feedforward control 00736 float temp_vel = 0.0f; 00737 00738 float wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control 00739 00740 pos.err = pos.ref - pos.sen; // Unit : mm or deg 00741 vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s 00742 00743 // position control command =============================================================================================================================================== 00744 if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode 00745 temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s 00746 // L when P-gain = 100, f_cut = 10Hz 00747 } else { 00748 temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s 00749 // L when P-gain = 100, f_cut = 10Hz 00750 } 00751 00752 // torque control command =============================================================================================================================================== 00753 float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*30.0f)); 00754 K_LPF = alpha_SpringDamper * K_LPF + (1.0f-alpha_SpringDamper) * K_SPRING; 00755 D_LPF = alpha_SpringDamper * D_LPF + (1.0f-alpha_SpringDamper) * D_DAMPER; 00756 00757 if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode 00758 float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm 00759 torq.err = torq_ref_act - torq.sen; 00760 torq.err_int += torq.err/((float)TMR_FREQ_5k); 00761 temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s 00762 } else { 00763 float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N 00764 force.err = force_ref_act - force.sen; 00765 force.err_int += force.err/((float)TMR_FREQ_5k); 00766 temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s 00767 } 00768 00769 // velocity feedforward command ======================================================================================================================================== 00770 if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode 00771 temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s 00772 } else { 00773 temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s 00774 } 00775 00776 // command integration ================================================================================================================================================= 00777 temp_vel = (1.0f - alpha_trans) * temp_vel_pos + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward 00778 00779 float Qact = 0.0f; // required flow rate 00780 if( temp_vel > 0.0f ) { 00781 Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM 00782 I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d; 00783 } else { 00784 Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM 00785 I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d; 00786 } 00787 00788 // Anti-windup for FT 00789 if (I_GAIN_JOINT_TORQUE != 0.0f) { 00790 float I_MAX = 10.0f; // Maximum Current : 10mA 00791 float Ka = 2.0f; 00792 if (I_REF > I_MAX) { 00793 float I_rem = I_REF - I_MAX; 00794 I_REF = I_MAX; 00795 float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary] 00796 torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! 00797 } else if (I_REF < -I_MAX) { 00798 double I_rem = I_REF - (-I_MAX); 00799 I_REF = -I_MAX; 00800 float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary] 00801 torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! 00802 } 00803 } 00804 break; 00805 } 00806 00807 case MODE_VALVE_OPEN_LOOP: { 00808 V_out = (float) Vout.ref; 00809 break; 00810 } 00811 00812 // case MODE_JOINT_ADAPTIVE_BACKSTEPPING: { 00813 // 00814 // float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 00815 // float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 00816 // 00817 // V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f 00818 // 00819 // //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 00820 // float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 00821 // 00822 // float g3_prime = 0.0f; 00823 // if (torq.sen > Amm*(Ps-Pt)*0.000001f) { 00824 // g3_prime = 1.0f; 00825 // } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { 00826 // g3_prime = -1.0f; 00827 // } else { 00828 // if ((value-VALVE_CENTER) > 0) { 00829 // g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); 00830 //// g3_prime = sqrt(Ps-Pt); 00831 // } else { 00832 // g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); 00833 //// g3_prime = sqrt(Ps-Pt); 00834 // } 00835 // } 00836 // float tau = 0.01f; 00837 // float K_valve = 0.0004f; 00838 // 00839 // float x_v = 0.0f; //x_v : -1~1 00840 // if(value>=VALVE_CENTER) { 00841 // x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); 00842 // } else { 00843 // x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); 00844 // } 00845 // float f4 = -x_v/tau; 00846 // float g4 = K_valve/tau; 00847 // 00848 // float torq_ref_dot = torq.ref_diff * 500.0f; 00849 // 00850 // pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] 00851 // vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] 00852 // pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] 00853 // 00854 // torq.err = torq.ref - torq.sen; //[N] 00855 // torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] 00856 // 00857 // float k3 = 2000.0f; //2000 //20000 00858 // float k4 = 10.0f; 00859 // float rho3 = 3.2f; 00860 // float rho4 = 10000000.0f; //25000000.0f; 00861 // float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime); 00862 // if (x_4_des > 1) x_4_des = 1; 00863 // else if (x_4_des < -1) x_4_des = -1; 00864 // 00865 // if (x_4_des > 0) { 00866 // valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER; 00867 // } else { 00868 // valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER; 00869 // } 00870 // 00871 // float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k; 00872 // x_4_des_old = x_4_des; 00873 // V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4; 00874 // 00875 // float rho_a = 0.00001f; 00876 // float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err); 00877 // a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k; 00878 // 00879 // if(a_hat > -3000000.0f) a_hat = -3000000.0f; 00880 // else if(a_hat < -30000000.0f) a_hat = -30000000.0f; 00881 // 00882 // break; 00883 // } 00884 00885 default: 00886 break; 00887 } 00888 00889 00890 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve 00891 00892 //////////////////////////////////////////////////////////////////////////// 00893 //////////////////////////// CURRENT CONTROL ////////////////////////////// 00894 //////////////////////////////////////////////////////////////////////////// 00895 if (CURRENT_CONTROL_MODE) { 00896 double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz 00897 I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; 00898 00899 if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA 00900 else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA 00901 else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA 00902 00903 I_ERR = I_REF_fil_DZ - (double)cur.sen; 00904 I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; 00905 00906 00907 // Moog Valve Current Control Gain 00908 double R_model = 500.0f; // ohm 00909 double L_model = 1.2f; 00910 double w0 = 2.0f * 3.14f * 150.0f; 00911 double KP_I = 0.1f * L_model*w0; 00912 double KI_I = 0.1f * R_model*w0; 00913 00914 // KNR Valve Current Control Gain 00915 if (((OPERATING_MODE & 0b110)>>1) == 1) { // KNR Valve 00916 R_model = 163.0f; // ohm 00917 L_model = 1.0f; 00918 w0 = 2.0f * 3.14f * 80.0f; 00919 KP_I = 1.0f * L_model*w0; 00920 KI_I = 0.08f * R_model*w0; 00921 } 00922 00923 double FF_gain = 1.0f; 00924 00925 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT; 00926 I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old; 00927 I_REF_fil_old = I_REF_fil_DZ; 00928 // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV 00929 VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV 00930 double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV 00931 00932 double Ka = 3.0f / KP_I; 00933 if (VALVE_PWM_RAW > V_MAX) { 00934 V_rem = VALVE_PWM_RAW - V_MAX; 00935 V_rem = Ka*V_rem; 00936 VALVE_PWM_RAW = V_MAX; 00937 I_ERR_INT = I_ERR_INT - V_rem * 0.0002f; 00938 } else if (VALVE_PWM_RAW < -V_MAX) { 00939 V_rem = VALVE_PWM_RAW - (-V_MAX); 00940 V_rem = Ka*V_rem; 00941 VALVE_PWM_RAW = -V_MAX; 00942 I_ERR_INT = I_ERR_INT - V_rem * 0.0002f; 00943 } 00944 } else { 00945 VALVE_PWM_RAW = I_REF * mV_PER_mA; 00946 } 00947 00948 //////////////////////////////////////////////////////////////////////////// 00949 ///////////////// Dead Zone Cancellation & Linearization ////////////////// 00950 //////////////////////////////////////////////////////////////////////////// 00951 00952 // Output Voltage Linearization 00953 double CUR_PWM_nonlin = (double)VALVE_PWM_RAW; // Unit : mV 00954 double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000 00955 00956 // Dead Zone Cancellation (Electrical dead-zone) 00957 if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f); 00958 else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f); 00959 else V_out = (float) (CUR_PWM_lin); 00960 00961 } else { //////////////////////////sw valve 00962 // Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW 00963 if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f; 00964 else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f; 00965 else V_out = 0.0f; 00966 } 00967 00968 //////////////////////////////////////////////////////////////////// 00969 /////////////////// PWM Command /////////////////////////////////// 00970 //////////////////////////////////////////////////////////////////// 00971 if(DIR_VALVE<0) { 00972 V_out = -V_out; 00973 } 00974 00975 if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) { 00976 V_out = VALVE_VOLTAGE_LIMIT*1000.0f; 00977 } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) { 00978 V_out = -VALVE_VOLTAGE_LIMIT*1000.0f; 00979 } 00980 PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); 00981 00982 // Saturation of output voltage 00983 if(PWM_out > 1.0f) PWM_out=1.0f; 00984 else if (PWM_out < -1.0f) PWM_out=-1.0f; 00985 00986 if (PWM_out>0.0f) { 00987 dtc_v=0.0f; 00988 dtc_w=PWM_out; 00989 } else { 00990 dtc_v=-PWM_out; 00991 dtc_w=0.0f; 00992 } 00993 00994 //pwm 00995 TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); 00996 TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); 00997 00998 //////////////////////////////////////////////////////////////////////////// 00999 ////////////////////// Data transmission through CAN ////////////////////// 01000 //////////////////////////////////////////////////////////////////////////// 01001 01002 if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { 01003 01004 // Position, Velocity, and Torque (ID:1200) 01005 if (flag_data_request[0] == HIGH) { 01006 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator 01007 CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); 01008 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator 01009 CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); 01010 } 01011 } 01012 01013 // Valve Position (ID:1300) 01014 if (flag_data_request[1] == HIGH) { 01015 CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); 01016 } 01017 01018 // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) 01019 if (flag_data_request[2] == HIGH) { 01020 CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM)); 01021 } 01022 01023 TMR2_COUNT_CAN_TX = 0; 01024 } 01025 TMR2_COUNT_CAN_TX++; 01026 01027 } 01028 TIM3->SR = 0x0; // reset the status register 01029 01030 }
Generated on Sun Jul 17 2022 16:20:45 by
1.7.2