![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 231:30896263bd8b
- Parent:
- 230:b235d67d25ba
- Child:
- 232:e9c5ec04e378
diff -r b235d67d25ba -r 30896263bd8b main.cpp --- a/main.cpp Thu Apr 07 06:18:38 2022 +0000 +++ b/main.cpp Mon Jun 13 08:48:55 2022 +0000 @@ -1,20 +1,12 @@ -//Hydraulic Control Board +//Hydraulic Control Board Rainbow //distributed by Sungwoo Kim -// 2020/12/28 -//revised by Buyoun Cho -// 2021/04/20 - -// 유의사항 -// 소수 적을때 뒤에 f 꼭 붙이기 -// CAN 선은 ground까지 있는 3상 선으로 써야함. -// 전원은 12~24V 인가. +// 2022/05/31 #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" @@ -23,39 +15,34 @@ #include <iostream> #include <cmath> -using namespace std; -Timer t; -// dac & check /////////////////////////////////////////// -DigitalOut check(PC_2); -DigitalOut check_2(PC_3); +// DAC /////////////////////////////////////////// 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 + +// ADC /////////////////////////////////////////// +//AnalogIn adc1(PC_4); //pressure_1 +//AnalogIn adc2(PC_5); //pressure_2 +//AnalogIn adc3(PC_1); //current +//AnalogIn adc4(PB_0); //LVDT // 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) -const int i2c_slave_addr1 = 0x56; // AS5510 address -unsigned int value; // 10bit output of reading sensor AS5510 +// LVDT /////////////////////////////////////////// +//DigitalOut LVDT_H(PB_6); +//DigitalOut LVDT_L(PB_7); // 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); + +// LED /////////////////////////////////////////// DigitalOut LED(PA_15); -// UART /////////////////////////////////////////// -Serial pc(PA_9,PA_10); // _ UART - // CAN /////////////////////////////////////////// CAN can(PB_8, PB_9, 1000000); CANMessage msg; @@ -77,11 +64,6 @@ 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; @@ -95,12 +77,6 @@ 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; @@ -167,12 +143,9 @@ 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; @@ -202,36 +175,26 @@ } + 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 + // SPI INIT eeprom_cs = 1; eeprom.format(8,3); eeprom.frequency(5000000); //5M eeprom_cs = 0; make_delay(); - enc_cs = 1; //sw add + enc_cs = 1; enc.format(8,0); enc.frequency(5000000); //10M - enc_cs = 0; //sw add + enc_cs = 0; make_delay(); @@ -239,7 +202,7 @@ spi_enc_set_init(); make_delay(); - ////// bno rom + // bno rom spi_eeprom_write(RID_BNO, (int16_t) 0); make_delay(); //////// @@ -263,25 +226,41 @@ make_delay(); //can.reset(); -// can.filter(msg.id, 0xFFFFF000, CANStandard); -// can.filter(0x050, 0xFFFFFFFF, CANStandard); - can.filter(0b100000000, 0b100000010, CANStandard); //CAN ID 100~400번대 통과하게 + can.filter(msg.id, 0xFFFFF000, CANStandard); +// can.filter(0b100000000, 0b100000010, CANStandard); //CAN ID 100~400번대 통과하게 // TMR3 init Init_TMR3(); TIM3->CR1 ^= TIM_CR1_UDIS; make_delay(); - // TMR5 init + // TMR2 init Init_TMR2(); TIM2->CR1 ^= TIM_CR1_UDIS; make_delay(); + // TMR1 init + Init_TMR1(); + TIM1->CR1 ^= TIM_CR1_UDIS; + make_delay(); + //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); NVIC_SetPriority(TIM4_IRQn, 3); NVIC_SetPriority(TIM2_IRQn, 4); +// HAL_NVIC_SetPriority(TIM2_IRQn, 4, 0); +// HAL_NVIC_EnableIRQ(TIM2_IRQn); +// /* TIM3_IRQn interrupt configuration */ +// HAL_NVIC_SetPriority(TIM3_IRQn, 2, 0); +// HAL_NVIC_EnableIRQ(TIM3_IRQn); +// /* TIM4_IRQn interrupt configuration */ +// HAL_NVIC_SetPriority(TIM4_IRQn, 3, 0); +// HAL_NVIC_EnableIRQ(TIM4_IRQn); +// /* CAN1_RX0_IRQn interrupt configuration */ +// HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0); +// HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + //DAC init if (SENSING_MODE == 0) { @@ -305,29 +284,16 @@ 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! *************************************/ while(1) { + +// if (LED > 0) LED = 0; +// else LED = 1; + TIM1->CCR1 = (TMR1_COUNT)*(0.7f); - // UART example -// if(timer_while==100000) { -// timer_while = 0; -// pc.printf("%f\n", value); -// } -// timer_while ++; - //i2c for SW valve -// if(OPERATING_MODE == 5) { -// read_field(i2c_slave_addr1); -// if(DIR_VALVE_ENC < 0) value = 1023 - value; -// } } } @@ -375,7 +341,7 @@ } else if(REF_VALVE_POS < VALVE_MIN_POS) { REF_VALVE_POS = VALVE_MIN_POS; } - valve_pos_err = (float) (REF_VALVE_POS - value); + valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen); valve_pos_err_diff = valve_pos_err - valve_pos_err_old; valve_pos_err_old = valve_pos_err; valve_pos_err_sum += valve_pos_err; @@ -473,7 +439,6 @@ } } } - return PWM_duty; } @@ -483,43 +448,65 @@ TIMER INTERRUPT *******************************************************************************/ + +//------------------------------------------------ +// TMR3 : LVDT 1kHz +//----------------------------------------------- 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; +//// if (LED > 0) LED = 0; +//// else LED = 1; +// +// LVDT_sum = 0.0f; +// +// +// LVDT_L = 0; +// LVDT_H = 1; +// +//// LED = 1; +// +// for (int ij = 0; ij<150; ij++) { +// ADC1->CR2 |= 0x40000000; +// LVDT_new = ((float)ADC1->DR) - 2047.5f; +// LVDT_sum = LVDT_sum + LVDT_new; +// } +// +//// LED = 0; +// +// LVDT_H = 0; +// LVDT_L = 1; - LVDT_sum = 0.0f; - - LVDT_H = 1; - LVDT_L = 0; - for (int ij = 0; ij<200; ij++) { -// LED = 1; - ADC1->CR2 |= 0x40000000; - LVDT_new = ((float)ADC1->DR) - 2047.5f; - LVDT_sum = LVDT_sum + LVDT_new; -// LED = 0; - } + + +// LVDT_new = LVDT_sum * 0.01f*2.0f; +// +// float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f)); +// LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new; +// valve_pos.sen = LVDT_LPF; +// if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen; - LVDT_H = 0; - LVDT_L = 0; +// TIM4->CCR2 = (PWM_ARR)*(0.95f); +// TIM4->CCR1 = (PWM_ARR)*(0.975f); + +// TIM8->CCR1 = (TMR8_COUNT)*(0.95f); +// TIM8->CCR2 = (TMR8_COUNT)*(0.975f); + -// LED = 0; - LVDT_new = LVDT_sum * 0.005f; +// PWM_H2 = 0; +// PWM_L2 = 1; - float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f)); - LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new; - valve_pos.sen = LVDT_LPF; - if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen; +// TIM3->CCR2 = (TMR3_COUNT)*(0.95f); +// TIM3->CCR1 = (TMR3_COUNT)*(0.975f); + } TIM3->SR = 0x0; // reset the status register @@ -538,9 +525,6 @@ { if (TIM4->SR & TIM_SR_UIF ) { -// if (LED > 0) LED = 0; -// else LED = 1; - // Current =================================================== //ADC3->CR2 |= 0x40000000; // adc _ 12bit @@ -578,6 +562,51 @@ } */ +// //FORWARD +// if(CNT_TMR4 < 2) +// { +// PWM_L2 = 0; +// PWM_H1 = 0; +// +// PWM_H2 = 1; +// PWM_L1 = 1; +// } else if(CNT_TMR4 < 50) +// { +// PWM_H2 = 0; +// PWM_H1 = 0; +// +// PWM_L2 = 1; +// PWM_L1 = 1; +// } else +// { +// CNT_TMR4 = 0; +// } + + +// //BACKWARD +// if(CNT_TMR4 < 2) +// { +// PWM_H2 = 0; +// PWM_L1 = 0; +// +// PWM_L2 = 1; +// PWM_H1 = 1; +// } else if(CNT_TMR4 < 50) +// { +// PWM_H2 = 0; +// PWM_H1 = 0; +// +// PWM_L2 = 1; +// PWM_L1 = 1; +// } else +// { +// CNT_TMR4 = 0; +// } + + + + + CNT_TMR4++; } TIM4->SR = 0x0; // reset the status register @@ -588,7 +617,6 @@ 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; @@ -828,7 +856,6 @@ data_num = 0; } else if(VALVE_ID_timer < TMR_FREQ_5k*3) { data_num = data_num + 1; -// VALVE_POS_TMP = VALVE_POS_TMP + value; VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen; } else if(VALVE_ID_timer == TMR_FREQ_5k*3) { Vout.ref = 0.0f; @@ -884,7 +911,7 @@ } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) { Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; data_num = data_num + 1; - VALVE_POS_TMP = VALVE_POS_TMP + value; + VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen; } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) { Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; DDV_POS_AVG = VALVE_POS_TMP / data_num; @@ -923,7 +950,7 @@ } } else { if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) { - if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) { + if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; @@ -1096,18 +1123,18 @@ valve_pos_ref = 10000.0f; } 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; + valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)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_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)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); + if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) { + pos_array[cnt_step_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER); } else { - pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER); + pos_array[cnt_step_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER); } CONTROL_MODE = MODE_VALVE_OPEN_LOOP; @@ -1119,17 +1146,10 @@ CONTROL_UTILITY_MODE = MODE_SEND_OVER; CONTROL_MODE = MODE_NO_ACT; } -// if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) -// { -// CONTROL_UTILITY_MODE = MODE_NO_ACT; -// CONTROL_MODE = MODE_NO_ACT; -// CAN_TX_PWM((int16_t) (1)); //1300 -// } break; } case MODE_SEND_OVER: { - CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300 CONTROL_UTILITY_MODE = MODE_NO_ACT; CONTROL_MODE = MODE_NO_ACT; break; @@ -1138,18 +1158,21 @@ case MODE_FREQ_TEST: { 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; + valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)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_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)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); +// if(value>=(float) VALVE_ELECTRIC_CENTER) { + if(valve_pos.sen>=(float) VALVE_ELECTRIC_CENTER) { +// pos_array[cnt_freq_test] = 10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER); + pos_array[cnt_freq_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER); } else { - pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER); +// pos_array[cnt_freq_test] = -10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER); + pos_array[cnt_freq_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER); } CONTROL_MODE = MODE_VALVE_OPEN_LOOP; @@ -1162,7 +1185,6 @@ if (freq_test_valve_ref >= 400) { CONTROL_UTILITY_MODE = MODE_NO_ACT; CONTROL_MODE = MODE_NO_ACT; - CAN_TX_VOUT((int16_t) (1)); //1300 } CONTROL_MODE = MODE_NO_ACT; CONTROL_UTILITY_MODE = MODE_SEND_OVER; @@ -1281,7 +1303,6 @@ float I_MAX = 10.0f; // Maximum Current : 10mA // Anti-windup for FT - // if (I_GAIN_JOINT_TORQUE != 0.0f) { if (I_GAIN_JOINT_TORQUE > 0.001f) { float Ka = 2.0f; if (I_REF > I_MAX) { @@ -1305,7 +1326,6 @@ } else { //SW valve float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000 // Anti-windup for FT - // if (I_GAIN_JOINT_TORQUE != 0.0f) { if (I_GAIN_JOINT_TORQUE > 0.001f) { float Ka = 2.0f; if (valve_pos_pulse > Valve_pos_MAX) { @@ -1332,7 +1352,6 @@ // 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; @@ -1343,79 +1362,6 @@ break; } -// case MODE_JOINT_ADAPTIVE_BACKSTEPPING: { -// -// 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 -// 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 -// -// V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f -// -// //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 -// float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 -// -// float g3_prime = 0.0f; -// if (torq.sen > Amm*(Ps-Pt)*0.000001f) { -// g3_prime = 1.0f; -// } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { -// g3_prime = -1.0f; -// } else { -// if ((value-VALVE_CENTER) > 0) { -// g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); -//// g3_prime = sqrt(Ps-Pt); -// } else { -// g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); -//// g3_prime = sqrt(Ps-Pt); -// } -// } -// float tau = 0.01f; -// float K_valve = 0.0004f; -// -// float x_v = 0.0f; //x_v : -1~1 -// if(value>=VALVE_CENTER) { -// x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); -// } else { -// x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); -// } -// float f4 = -x_v/tau; -// float g4 = K_valve/tau; -// -// float torq_ref_dot = torq.ref_diff * 500.0f; -// -// pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] -// vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] -// pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] -// -// torq.err = torq.ref - torq.sen; //[N] -// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] -// -// float k3 = 2000.0f; //2000 //20000 -// float k4 = 10.0f; -// float rho3 = 3.2f; -// float rho4 = 10000000.0f; //25000000.0f; -// float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime); -// if (x_4_des > 1) x_4_des = 1; -// else if (x_4_des < -1) x_4_des = -1; -// -// if (x_4_des > 0) { -// valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER; -// } else { -// valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER; -// } -// -// float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k; -// x_4_des_old = x_4_des; -// V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4; -// -// float rho_a = 0.00001f; -// float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err); -// a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k; -// -// if(a_hat > -3000000.0f) a_hat = -3000000.0f; -// else if(a_hat < -30000000.0f) a_hat = -30000000.0f; -// -// break; -// } - default: break; } @@ -1437,7 +1383,6 @@ I_ERR = I_REF_fil_DZ - (double)cur.sen; I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; - // Moog Valve Current Control Gain double R_model = 500.0f; // ohm double L_model = 1.2f; @@ -1455,7 +1400,6 @@ } double FF_gain = 1.0f; - VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT; I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old; I_REF_fil_old = I_REF_fil_DZ; @@ -1527,54 +1471,38 @@ 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) { +// if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) { // Position, Velocity, and Torque (ID:1200) -// if (flag_data_request[0] == LOW) { + if (flag_data_request[0] == HIGH) { -// 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)); + 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)); -// } 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) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen)); -// } -// } + } 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)); + } + } // 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)); + if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve + CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); + } else { + CAN_TX_PWM((int16_t)(valve_pos.sen)); + } } - // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) + // Others : SW (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); + CAN_TX_CURRENT((int16_t) valve_pos.sen, (int16_t) valve_pos.ref); } TMR2_COUNT_CAN_TX = 0; @@ -1583,5 +1511,4 @@ } TIM2->SR = 0x0; // reset the status register - -} \ No newline at end of file +}