Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy1
2011
Diff: main.cpp
- Revision:
- 240:fb5b57e3f157
- Parent:
- 239:8ac5c6162bc1
- Child:
- 241:27eca07c9591
diff -r 8ac5c6162bc1 -r fb5b57e3f157 main.cpp --- a/main.cpp Fri Jul 30 06:04:10 2021 +0000 +++ b/main.cpp Tue Apr 05 05:12:12 2022 +0000 @@ -38,6 +38,8 @@ // PWM /////////////////////////////////////////// float dtc_v=0.0f; float dtc_w=0.0f; +DigitalOut LVDT_H(PB_4); //PWM_H +DigitalOut LVDT_L(PB_5); //PWM_L // I2C /////////////////////////////////////////// I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F) @@ -97,6 +99,7 @@ float temp_I_GAIN = 0.0f; int temp_VELOCITY_COMP_GAIN = 0; int logging = 0; +float valve_pos_pulse_can = 0.0f; inline float tanh_inv(float y) { @@ -170,13 +173,13 @@ __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); /* Initializes the CPU, AHB and APB busses clocks */ - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; - RCC_OscInitStruct.HSIState = RCC_HSI_ON; - RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; +// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; - RCC_OscInitStruct.PLL.PLLM = 8;//8 - RCC_OscInitStruct.PLL.PLLN = 180; //180 + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.PLL.PLLM = 4;//4 + RCC_OscInitStruct.PLL.PLLN = 96; //96 RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; RCC_OscInitStruct.PLL.PLLQ = 2; RCC_OscInitStruct.PLL.PLLR = 2; @@ -196,10 +199,6 @@ RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; - - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { - //Error_Handler(); - } } @@ -241,7 +240,7 @@ make_delay(); ////// bno rom - spi_eeprom_write(RID_BNO, (int16_t) 1); + spi_eeprom_write(RID_BNO, (int16_t) 0); make_delay(); //////// @@ -271,9 +270,15 @@ TIM3->CR1 ^= TIM_CR1_UDIS; make_delay(); + // TMR5 init + Init_TMR2(); + TIM2->CR1 ^= TIM_CR1_UDIS; + make_delay(); + //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); NVIC_SetPriority(TIM4_IRQn, 3); + NVIC_SetPriority(TIM2_IRQn, 4); //DAC init @@ -282,8 +287,8 @@ dac_2 = 0.0f; } else if (SENSING_MODE == 1) { // if (DIR_VALVE_ENC > 0) { - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; // } else { // dac_1 = PRES_B_VREF / 3.3f; // dac_2 = PRES_A_VREF / 3.3f; @@ -298,6 +303,12 @@ ID_index_array[i] = (i+1) * 0.5f; } + //pwm + TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f); + TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f); + LVDT_H = 0; + LVDT_L = 0; + /************************************ *** Program is operating! *************************************/ @@ -468,6 +479,49 @@ TIMER INTERRUPT *******************************************************************************/ +float LVDT_new = 0.0f; +float LVDT_old = 0.0f; +float LVDT_f_cut = 1000.0f; +float LVDT_LPF = 0.0f; +float LVDT_sum = 0.0f; +float LVDT_can[100] = {0.0f}; + +extern "C" void TIM3_IRQHandler(void) +{ + if (TIM3->SR & TIM_SR_UIF ) { + +// if (LED > 0) LED = 0; +// else LED = 1; + + LVDT_sum = 0.0f; + + LVDT_H = 1; + LVDT_L = 0; + + for (int ij = 0; ij<400; ij++) { +// LED = 1; + ADC1->CR2 |= 0x40000000; + LVDT_new = ((float)ADC1->DR) - 2047.5f; + LVDT_sum = LVDT_sum + LVDT_new; +// LED = 0; + } + + LVDT_H = 0; + LVDT_L = 0; + +// LED = 0; + LVDT_new = LVDT_sum * 0.0025f; + + float alpha_LVDT = 1.0f;//1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f)); + LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new; + force.sen = LVDT_LPF; + + } + TIM3->SR = 0x0; // reset the status register +} + + + //------------------------------------------------ // TMR4 : Sensor Read & Data Handling //----------------------------------------------- @@ -479,6 +533,9 @@ { if (TIM4->SR & TIM_SR_UIF ) { +// if (LED > 0) LED = 0; +// else LED = 1; + // Current =================================================== //ADC3->CR2 |= 0x40000000; // adc _ 12bit @@ -489,19 +546,20 @@ ENC_UPDATE(); } + /* // Force or Pressure Transducer ============================================= ADC1->CR2 |= 0x40000000; if (SENSING_MODE == 0) { // Force sensing force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N } else if (SENSING_MODE == 1) { // Pressure sensing float pres_A_new, pres_B_new; -// if (DIR_VALVE_ENC > 0) { - pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar - pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; -// } else { -// pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar -// pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; -// } + // if (DIR_VALVE_ENC > 0) { + pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar + pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; + // } else { + // pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar + // pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; + // } pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f); pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f); @@ -513,6 +571,7 @@ force.UpdateSen(force_new,FREQ_TMR4,1000.0f); // unit : N } } + */ CNT_TMR4++; } @@ -521,32 +580,35 @@ int j =0; -float FREQ_TMR3 = (float)FREQ_5k; -float DT_TMR3 = (float)DT_5k; +float FREQ_TMR5 = (float)FREQ_5k; +float DT_TMR5 = (float)DT_5k; int cnt_trans = 0; double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f; int can_rest =0; float force_ref_act_can = 0.0f; -extern "C" void TIM3_IRQHandler(void) +extern "C" void TIM2_IRQHandler(void) { - if (TIM3->SR & TIM_SR_UIF ) { + if (TIM2->SR & TIM_SR_UIF ) { + + if (LED > 0) LED = 0; + else LED = 1; if(MODE_POS_FT_TRANS == 1) { if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; - alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; + alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR5 /3.0f))/2.0f; cnt_trans++; torq.err_int = 0.0f; force.err_int = 0.0f; - if((float)cnt_trans * DT_TMR3 > 3.0f) + if((float)cnt_trans * DT_TMR5 > 3.0f) MODE_POS_FT_TRANS = 2; } else if(MODE_POS_FT_TRANS == 3) { if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0; - alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; + alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR5 /3.0f))/2.0f; cnt_trans++; torq.err_int = 0.0f; force.err_int = 0.0f; - if((float) cnt_trans * DT_TMR3 > 3.0f ) + if((float) cnt_trans * DT_TMR5 > 3.0f ) MODE_POS_FT_TRANS = 0; } else if(MODE_POS_FT_TRANS == 2) { alpha_trans = 1.0f; @@ -1032,9 +1094,9 @@ } else { valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; } - + VALVE_POS_CONTROL(valve_pos_raw.ref); - + ref_array[cnt_step_test] = valve_pos_ref; if(value>=(float) VALVE_ELECTRIC_CENTER) { pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER); @@ -1068,15 +1130,15 @@ } case MODE_FREQ_TEST: { - float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3); + float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5); if(valve_pos_ref >= 0) { valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; } else { valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; } - + VALVE_POS_CONTROL(valve_pos_raw.ref); - + ref_array[cnt_freq_test] = valve_pos_ref; if(value>=(float) VALVE_ELECTRIC_CENTER) { pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER); @@ -1086,7 +1148,7 @@ CONTROL_MODE = MODE_VALVE_OPEN_LOOP; cnt_freq_test++; - if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) { + if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5 > 2) { buffer_data_size = cnt_freq_test; cnt_freq_test = 0; cnt_send_buffer = 0; @@ -1166,11 +1228,11 @@ } else { float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N //////////////////////////////////////////////////force_reference_filter//////////////////////////////////////////////////////////////////// - float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f)); - force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act; +// float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f)); +// force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - force_ref_act_can = force_ref_filter; - force.err = force_ref_filter - force.sen; + force_ref_act_can = force_ref_act; + force.err = force_ref_act - force.sen; force.err_int += force.err/((float)TMR_FREQ_5k); temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s } @@ -1264,6 +1326,7 @@ // valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; // } VALVE_POS_CONTROL_DZ(valve_pos_pulse); + valve_pos_pulse_can = valve_pos_pulse; V_out = Vout.ref; } break; @@ -1477,14 +1540,14 @@ } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); -// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (valve_pos_can*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); +// CAN_TX_POSITION_FT((int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f), (int16_t) (vel.sen*20.0f), (int16_t) (((float)ADC1->DR))); } } // Valve Position (ID:1300) if (flag_data_request[1] == HIGH) { CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); -// CAN_TX_PWM((int16_t)(alpha3)); +// CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f)); } // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) @@ -1504,7 +1567,7 @@ valve_pos_ref_can = (float)valve_pos.ref; - CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) force_ref_act_can); + CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can); } TMR2_COUNT_CAN_TX = 0; @@ -1512,6 +1575,6 @@ TMR2_COUNT_CAN_TX++; } - TIM3->SR = 0x0; // reset the status register + TIM2->SR = 0x0; // reset the status register } \ No newline at end of file