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