Sungwoo Kim
2011
Diff: main.cpp
- Revision:
- 228:fb5b57e3f157
- Parent:
- 227:8ac5c6162bc1
- Child:
- 229:27eca07c9591
--- 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