Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
main.cpp
- Committer:
- Lightvalve
- Date:
- 2022-03-31
- Revision:
- 257:c93d3eabff75
- Parent:
- 239:8ac5c6162bc1
File content as of revision 257:c93d3eabff75:
//Hydraulic Control Board //distributed by Sungwoo Kim // 2020/12/28 //revised by Buyoun Cho // 2021/04/20 // 유의사항 // 소수 적을때 뒤에 f 꼭 붙이기 // CAN 선은 ground까지 있는 3상 선으로 써야함. // 전원은 12~24V 인가. #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" #include "function_CAN.h" #include "SPI_EEP_ENC.h" #include "I2C_AS5510.h" #include "setting.h" #include "function_utilities.h" #include "stm32f4xx_flash.h" #include "FlashWriter.h" #include <string> #include <iostream> #include <cmath> using namespace std; Timer t; // dac & check /////////////////////////////////////////// DigitalOut check(PC_2); DigitalOut check_2(PC_3); AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f AnalogIn adc1(PC_4); //pressure_1 AnalogIn adc2(PB_0); //pressure_2 AnalogIn adc3(PC_1); //current // PWM /////////////////////////////////////////// float dtc_v=0.0f; float dtc_w=0.0f; // I2C /////////////////////////////////////////// I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F) const int i2c_slave_addr1 = 0x56; // AS5510 address unsigned int value; // 10bit output of reading sensor AS5510 // SPI /////////////////////////////////////////// SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK); DigitalOut eeprom_cs(PB_12); SPI enc(PC_12,PC_11,PC_10); DigitalOut enc_cs(PD_2); DigitalOut LED(PA_15); // UART /////////////////////////////////////////// Serial pc(PA_9,PA_10); // _ UART // CAN /////////////////////////////////////////// CAN can(PB_8, PB_9, 1000000); CANMessage msg; void onMsgReceived() { CAN_RX_HANDLER(); } // Variables /////////////////////////////////////////// State pos; State vel; State Vout; State force; State torq; // unit : N State torq_dot; State pres_A; // unit : bar State pres_B; State cur; // unit : mA State valve_pos; State valve_pos_raw; State INIT_Vout; State INIT_Valve_Pos; State INIT_Pos; State INIT_torq; extern int CID_RX_CMD; extern int CID_RX_REF_POSITION; extern int CID_RX_REF_OPENLOOP; extern int CID_RX_REF_PWM; extern int CID_TX_INFO; extern int CID_TX_POS_VEL_TORQ; extern int CID_TX_PWM; extern int CID_TX_CURRENT; extern int CID_TX_VOUT; extern int CID_TX_VALVE_POSITION; extern int CID_TX_SOMETHING; float temp_P_GAIN = 0.0f; 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) { if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f; if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f; return log(sqrt((1.0f+y)/(1.0f-y))); } /******************************************************************************* * REFERENCE MODE ******************************************************************************/ enum _REFERENCE_MODE { MODE_REF_NO_ACT = 0, MODE_REF_DIRECT, MODE_REF_FINDHOME }; /******************************************************************************* * CONTROL MODE ******************************************************************************/ enum _CONTROL_MODE { //control mode MODE_NO_ACT = 0, //0 MODE_VALVE_POSITION_CONTROL, //1 MODE_JOINT_CONTROL, //2 MODE_VALVE_OPEN_LOOP, //3 MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4 MODE_RL, //5 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7 MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8 MODE_TEST_CURRENT_CONTROL, //9 MODE_TEST_PWM_CONTROL, //10 MODE_CURRENT_CONTROL, //11 MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12 MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //14 //utility MODE_TORQUE_SENSOR_NULLING = 20, //20 MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21 MODE_FIND_HOME, //22 MODE_VALVE_GAIN_SETTING, //23 MODE_PRESSURE_SENSOR_NULLING, //24 MODE_PRESSURE_SENSOR_CALIB, //25 MODE_ROTARY_FRICTION_TUNING, //26 MODE_DDV_POS_VS_PWM_ID = 30, //30 MODE_DDV_DEADZONE_AND_CENTER, //31 MODE_DDV_POS_VS_FLOWRATE, //32 MODE_SYSTEM_ID, //33 MODE_FREQ_TEST, //34 MODE_SEND_BUFFER, //35 MODE_SEND_OVER, //36 MODE_STEP_TEST, //37 }; void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /* Configure the main internal regulator output voltage */ __HAL_RCC_PWR_CLK_ENABLE(); __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); /* Initializes the CPU, AHB and APB busses clocks */ 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_HSE; RCC_OscInitStruct.PLL.PLLM = 1;//4 RCC_OscInitStruct.PLL.PLLN = 192; //96 RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; RCC_OscInitStruct.PLL.PLLQ = 2; RCC_OscInitStruct.PLL.PLLR = 2; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { //Error_Handler(); } /** Activate the Over-Drive mode */ if (HAL_PWREx_EnableOverDrive() != HAL_OK) { //Error_Handler(); } /** Initializes the CPU, AHB and APB busses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { //Error_Handler(); } // HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5); // HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); // HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); // HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); } //void SystemClock_Config(void) //External clock //{ // RCC_OscInitTypeDef RCC_OscInitStruct; // RCC_ClkInitTypeDef RCC_ClkInitStruct; // // /* Configure the main internal regulator output voltage // */ // __PWR_CLK_ENABLE(); // __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); // /* Initializes the CPU, AHB and APB busses clocks // */ // RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; // RCC_OscInitStruct.HSIState = RCC_HSE_ON; //// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; // RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; // RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; // RCC_OscInitStruct.PLL.PLLM = 8;//8 // RCC_OscInitStruct.PLL.PLLN = 80; //180 // RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; // RCC_OscInitStruct.PLL.PLLQ = 4; // HAL_RCC_OscConfig(&RCC_OscInitStruct); // // HAL_PWREx_ActivateOverDrive(); //// RCC_OscInitStruct.PLL.PLLR = 2; //// if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { //// //Error_Handler(); //// } //// /** Activate the Over-Drive mode //// */ //// if (HAL_PWREx_EnableOverDrive() != HAL_OK) { //// //Error_Handler(); //// } // /** Initializes the CPU, AHB and APB busses clocks // */ // RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK // |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; // RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; // RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; // RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; // HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5); // // HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); // // HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); // // HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); //} int main() { /********************************* *** Initialization *********************************/ HAL_Init(); SystemClock_Config(); LED = 0; // pc.baud(9600); // // // i2c init // i2c.frequency(400 * 1000); // 0.4 mHz // wait_ms(2); // Power Up wait // look_for_hardware_i2c(); // Hardware present // init_as5510(i2c_slave_addr1); // make_delay(); // spi init // eeprom_cs = 1; // eeprom.format(8,3); // eeprom.frequency(5000000); //5M // eeprom_cs = 0; // make_delay(); // // enc_cs = 1; //sw add // enc.format(8,0); // enc.frequency(5000000); //10M // enc_cs = 0; //sw add // // make_delay(); // // // spi _ enc // spi_enc_set_init(); // make_delay(); // // ////// bno rom // spi_eeprom_write(RID_BNO, (int16_t) 0); // make_delay(); //////// // rom ROM_CALL_DATA(); make_delay(); // ADC init Init_ADC(); make_delay(); // Pwm init Init_PWM(); TIM4->CR1 ^= TIM_CR1_UDIS; make_delay(); // // CAN // can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); make_delay(); //can.reset(); // can.filter(msg.id, 0xFFFFF000, CANStandard); // // TMR2 init // Init_TMR2(); // TIM2->CR1 ^= TIM_CR1_UDIS; // make_delay(); // TMR3 init Init_TMR3(); TIM3->CR1 ^= TIM_CR1_UDIS; make_delay(); // //Timer priority NVIC_SetPriority(TIM3_IRQn, 3); NVIC_SetPriority(TIM4_IRQn, 2); // NVIC_SetPriority(TIM2_IRQn, 2); // // // //DAC init // if (SENSING_MODE == 0) { // dac_1 = FORCE_VREF / 3.3f; // 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; // // } else { // // dac_1 = PRES_B_VREF / 3.3f; // // dac_2 = PRES_A_VREF / 3.3f; // // } // } // make_delay(); TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f); TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f); /************************************ *** Program is operating! *************************************/ while(1) { // if (LED > 0) LED = 0; // else LED = 1; // ADC1->CR2 |= 0x40000000; // LVDT_new = ((float)ADC1->DR) - 2047.5f; // TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f); // TIM4->CCR1 = (PWM_ARR)*(1.0f-0.3f); } } //------------------------------------------------ // TMR4 : Sensor Read & Data Handling //----------------------------------------------- float FREQ_TMR4 = (float)FREQ_20k; float DT_TMR4 = (float)DT_20k; long CNT_TMR4 = 0; int TMR4_FREQ_20k = (int)FREQ_20k; //int toggle = 0; int PWM_Flag = 0; int TMR4_timer = 0; 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; extern "C" void TIM4_IRQHandler(void) { if (TIM4->SR & TIM_SR_UIF ) { // if (LED > 0) LED = 0; // else LED = 1; PWM_Flag++; if(PWM_Flag <= 15) { if (PWM_Flag >= 4 && PWM_Flag <=13) { ADC1->CR2 |= 0x40000000; LVDT_new = ((float)ADC1->DR) - 2047.5f; // if (ADC1->SR &= ~(ADC_SR_EOC)) { // if (LED > 0) LED = 0; // else LED = 1; // LED = 1; // } LVDT_sum = LVDT_sum + LVDT_new; if (LED > 0) LED = 0; else LED = 1; } //pwm TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f); TIM4->CCR1 = (PWM_ARR)*(1.0f-1.0f); } else if(PWM_Flag <= 500) { TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f); TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f); } else { PWM_Flag = 0; LVDT_LPF = 0.0f; force.sen = LVDT_sum * 0.1f; LVDT_sum = 0.0f; } CNT_TMR4++; } TIM4->SR = 0x0; // reset the status register } int j =0; float FREQ_TMR3 = (float)FREQ_5k; float DT_TMR3 = (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) { if (TIM3->SR & TIM_SR_UIF ) { // // ===================================================================== // // CONTROL LOOP -------------------------------------------------------- // // ===================================================================== // int UTILITY_MODE = 0; // int CONTROL_MODE = 0; // // if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) { // UTILITY_MODE = CONTROL_UTILITY_MODE; // CONTROL_MODE = MODE_NO_ACT; // } else { // CONTROL_MODE = CONTROL_UTILITY_MODE; // UTILITY_MODE = MODE_NO_ACT; // } // // UTILITY MODE ------------------------------------------------------------ // switch (UTILITY_MODE) { // case MODE_NO_ACT: { // break; // } // // case MODE_TORQUE_SENSOR_NULLING: { // static float FORCE_pulse_sum = 0.0; // static float PresA_pulse_sum = 0.0; // static float PresB_pulse_sum = 0.0; // // // DAC Voltage reference set // float VREF_TuningGain = -0.00000003f; // if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) { // if(SENSING_MODE == 0) { // Force Sensor (Loadcell) // FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE; // if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { // float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f; // FORCE_pulse_sum = 0.0f; // // FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean); // if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f; // if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f; // dac_1 = FORCE_VREF / 3.3f; // } // } else if (SENSING_MODE == 1) { // Pressure Sensor // PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR; // PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR; // if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { // float PresA_pluse_mean = PresA_pulse_sum / 10.0f; // float PresB_pluse_mean = PresB_pulse_sum / 10.0f; // PresA_pulse_sum = 0.0f; // PresB_pulse_sum = 0.0f; // // PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean); // if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; // if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; // dac_1 = PRES_A_VREF / 3.3f; // PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean); // if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; // if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; // dac_2 = PRES_B_VREF / 3.3f; // } // } // TMR3_COUNT_TORQUE_NULL++; // } else { // if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell) // FORCE_pulse_sum = 0.0f; // dac_1 = FORCE_VREF / 3.3f; // spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f)); // } else if (SENSING_MODE == 1) { // PresA_pulse_sum = 0.0f; // PresB_pulse_sum = 0.0f; // dac_1 = PRES_A_VREF / 3.3f; // dac_2 = PRES_B_VREF / 3.3f; // spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f)); // spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f)); // } // CONTROL_UTILITY_MODE = MODE_NO_ACT; // TMR3_COUNT_TORQUE_NULL = 0; // } // break; // } // // // default: // break; // } // // // CONTROL MODE ------------------------------------------------------------ // switch (CONTROL_MODE) { // case MODE_NO_ACT: { // V_out = 0.0f; // break; // } // // case MODE_VALVE_OPEN_LOOP: { // V_out = (float) Vout.ref; // break; // } // // default: // break; // } // // if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f; // else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f; // else V_out = 0.0f; // // // //////////////////////////////////////////////////////////////////// // /////////////////// PWM Command /////////////////////////////////// // //////////////////////////////////////////////////////////////////// // // if(DIR_VALVE<0) { // V_out = -V_out; // } // // if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) { // V_out = VALVE_VOLTAGE_LIMIT*1000.0f; // } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) { // V_out = -VALVE_VOLTAGE_LIMIT*1000.0f; // } // PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // // // Saturation of output voltage // if(PWM_out > 1.0f) PWM_out=1.0f; // else if (PWM_out < -1.0f) PWM_out=-1.0f; // // if (PWM_out>0.0f) { // dtc_v=0.0f; // dtc_w=PWM_out; // } else { // dtc_v=-PWM_out; // dtc_w=0.0f; // } // //// //pwm //// TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); //// TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); // // //////////////////////////////////////////////////////////////////////////// // ////////////////////// Data transmission through CAN ////////////////////// // //////////////////////////////////////////////////////////////////////////// // // if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == LOW) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); // CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } 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)); // 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)); } } // 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)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f)); } // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) if (flag_data_request[2] == HIGH) { float valve_pos_can = 0.0f; if(value >= VALVE_ELECTRIC_CENTER) { valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER); } else { valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); } float valve_pos_ref_can = 0.0f; if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) { valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER); } else { valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); } valve_pos_ref_can = (float)valve_pos.ref; CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can); } TMR2_COUNT_CAN_TX = 0; // } TMR2_COUNT_CAN_TX++; } TIM3->SR = 0x0; // reset the status register } // //float FREQ_TMR2 = (float)FREQ_400k; //float DT_TMR2 = (float)DT_400k; //int TMR2_timer = 0; //int toggle_old = 0; // //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; // //extern "C" void TIM2_IRQHandler(void) //{ // if (TIM2->SR & TIM_SR_UIF ) { //// if (LED > 0) LED = 0; //// else LED = 1; //// LED = 1; // // if (toggle != toggle_old) { // TMR2_timer = 0; // LVDT_LPF = 0.0f; // LVDT_sum = 0.0f; //// if (LED > 0) LED = 0; //// else LED = 1; // } // // if (TMR2_timer >= 6 && TMR2_timer <=45) { //// if (TMR2_timer >= 6 && TMR2_timer <=25) { //// if (LED > 0) LED = 0; //// else LED = 1; //// LED = 0; // ADC1->CR2 |= 0x40000000; // LVDT_new = ((float)ADC1->DR) - 2047.5f; // if (ADC1->SR &= ~(ADC_SR_EOC)) { //// if (LED > 0) LED = 0; //// else LED = 1; //// LED = 1; // } //// float alpha_update = 1.0f / (1.0f + FREQ_TMR2 / (2.0f * 3.14f * LVDT_f_cut)); // f_cutoff : 100Hz //// LVDT_LPF = (1.0f - alpha_update) * LVDT_LPF + alpha_update * LVDT_new; //// LVDT_sum = LVDT_sum + LVDT_LPF; // LVDT_sum = LVDT_sum + LVDT_new; // //// force.UpdateSen((((float)ADC1->DR) - 2047.5f), FREQ_TMR2, 100.0f); // unit : N //// this->sen_diff = (sen_new-this->sen)*Freq_update; //// float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz //// this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new; //// force.sen = LVDT_new; // } // // else if (TMR2_timer == 46) { //46 //// if (TMR2_timer == 50) { //// LED = 1; //// ADC1->CR2 |= 0x40000000; //// LVDT_new = ((float)ADC1->DR) - 2047.5f; //// force.sen = LVDT_new; // force.sen = LVDT_sum * 0.025f; //// force.sen = LVDT_sum; // //// ADC1->CR2 |= 0x40000000; //// LVDT_new = ((float)ADC1->DR) - 2047.5f; //// force.sen = LVDT_new; //// force.UpdateSen(LVDT_sum * 0.025f, 1000.0f, 10.0f); //// LED = 0; // } //// LED = 0; // toggle_old = toggle; // TMR2_timer++; // } // TIM2->SR = 0x0; // reset the status register // //}