Sungwoo Kim
2011
Diff: main.cpp
- Revision:
- 244:e9c5ec04e378
- Parent:
- 243:30896263bd8b
- Child:
- 245:071785d74ad0
diff -r 30896263bd8b -r e9c5ec04e378 main.cpp
--- a/main.cpp Mon Jun 13 08:48:55 2022 +0000
+++ b/main.cpp Fri Jun 17 06:06:53 2022 +0000
@@ -21,18 +21,18 @@
AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
// ADC ///////////////////////////////////////////
-//AnalogIn adc1(PC_4); //pressure_1
-//AnalogIn adc2(PC_5); //pressure_2
-//AnalogIn adc3(PC_1); //current
-//AnalogIn adc4(PB_0); //LVDT
+ADC_HandleTypeDef hadc1;
+ADC_HandleTypeDef hadc2;
+ADC_HandleTypeDef hadc3;
+
+AnalogIn adc1(PC_4); //pressure_1
+AnalogIn adc2(PC_5); //pressure_2
+AnalogIn adc3(PC_1); //current
+AnalogIn adc4(PB_1); //V_EXI
+AnalogIn adc5(PA_1); //LVDT
// PWM ///////////////////////////////////////////
-float dtc_v=0.0f;
-float dtc_w=0.0f;
-
-// LVDT ///////////////////////////////////////////
-//DigitalOut LVDT_H(PB_6);
-//DigitalOut LVDT_L(PB_7);
+float PWM_duty = 0.0f;
// SPI ///////////////////////////////////////////
SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
@@ -43,6 +43,13 @@
// LED ///////////////////////////////////////////
DigitalOut LED(PA_15);
+// LVDT ///////////////////////////////////////////
+DigitalOut LVDT_H(PB_6);
+DigitalOut LVDT_L(PB_7);
+
+// MOTOR_ENA ///////////////////////////////////////////
+DigitalOut M_ENABLE(PA_2);
+
// CAN ///////////////////////////////////////////
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;
@@ -159,13 +166,9 @@
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;
@@ -178,7 +181,7 @@
int main()
{
-
+
HAL_Init();
SystemClock_Config();
@@ -191,7 +194,7 @@
eeprom_cs = 0;
make_delay();
- enc_cs = 1;
+ enc_cs = 1;
enc.format(8,0);
enc.frequency(5000000); //10M
enc_cs = 0;
@@ -211,13 +214,20 @@
ROM_CALL_DATA();
make_delay();
+
+
// ADC init
- Init_ADC();
- make_delay();
+ RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC3
+ RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
+ RCC->APB2ENR |= RCC_APB2ENR_ADC3EN; // clock for ADC1
- // Pwm init
- Init_PWM();
- TIM4->CR1 ^= TIM_CR1_UDIS;
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; // Enable clock for GPIOC
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // Enable clock for GPIOB
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOB
+
+ Init_ADC1();
+ Init_ADC2();
+ Init_ADC3();
make_delay();
// CAN
@@ -229,38 +239,31 @@
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();
-
- // TMR2 init
- Init_TMR2();
- TIM2->CR1 ^= TIM_CR1_UDIS;
- make_delay();
-
- // TMR1 init
+ // TMR1 init (PWM)
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);
+ // TMR2 init (Control)
+ Init_TMR2();
+ TIM2->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
+
+ // TMR3 init (Sensors)
+ Init_TMR3();
+ TIM3->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
-// 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);
+ // TIM4 init (LVDT)
+ Init_TIM4();
+ TIM4->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
+ //Timer priority
+ NVIC_SetPriority(TIM3_IRQn, 3);
+ NVIC_SetPriority(TIM2_IRQn, 4);
+ NVIC_SetPriority(TIM4_IRQn, 2);
+ HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0);
//DAC init
if (SENSING_MODE == 0) {
@@ -275,6 +278,10 @@
// dac_2 = PRES_A_VREF / 3.3f;
// }
}
+
+ dac_1 = 1.0f/3.3f;
+ dac_2 = 1.0f/3.3f;
+
make_delay();
for (int i=0; i<50; i++) {
@@ -284,16 +291,15 @@
ID_index_array[i] = (i+1) * 0.5f;
}
+ M_ENABLE = 1;
+
/************************************
*** Program is operating!
*************************************/
while(1) {
-
-// if (LED > 0) LED = 0;
-// else LED = 1;
- TIM1->CCR1 = (TMR1_COUNT)*(0.7f);
-
+// if (LED > 0) LED = 0;
+// else LED = 1;
}
}
@@ -450,64 +456,78 @@
//------------------------------------------------
-// TMR3 : LVDT 1kHz
-//-----------------------------------------------
+// TMR3 : PWM, Sensor 20kHz
+//------------------------------------------------
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 FREQ_TMR3 = (float)FREQ_20k;
+long CNT_TMR3 = 0;
+float DT_TMR3 = (float)DT_20k;
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_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;
+ if (LED > 0) LED = 0;
+ else LED = 1;
+
+ float PSEN1 = 0.0f;
+ float PSEN2 = 0.0f;
+ float CURRENT_SEN = 0.0f;
+
+ /////////////////////////Current////////////////////////////////////////////////////////////////////////////
+ HAL_ADC_Start(&hadc2);
+ HAL_ADC_PollForConversion(&hadc2, 1);
+ CURRENT_SEN = (float) HAL_ADC_GetValue(&hadc2);
+ cur.UpdateSen(((float)CURRENT_SEN-2047.5f)/2047.5f*10.0f, FREQ_TMR3, 500.0f); // unit : mA
+
+ /////////////////////////V_EXI////////////////////////////////////////////////////////////////////////////
+ HAL_ADC_Start(&hadc2);
+ HAL_ADC_PollForConversion(&hadc2, 1);
+ V_EXI = (float) HAL_ADC_GetValue(&hadc2);
+
+ /////////////////////////Encoder////////////////////////////////////////////////////////////////////////////
+// if (CNT_TMR1 % 2) == 0) {
+ ENC_UPDATE();
// }
-//
-//// LED = 0;
-//
-// LVDT_H = 0;
-// LVDT_L = 1;
-
-
+ /////////////////////////Force or Pressure//////////////////////////////////////////////////////////////////
+ if (SENSING_MODE == 0) { // Force sensing
+
+ HAL_ADC_Start(&hadc1);
+ HAL_ADC_PollForConversion(&hadc1, 1);
+ PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
+ force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N
-// 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;
+ } else if (SENSING_MODE == 1) { // Pressure sensing
-// TIM4->CCR2 = (PWM_ARR)*(0.95f);
-// TIM4->CCR1 = (PWM_ARR)*(0.975f);
+ HAL_ADC_Start(&hadc1);
+ HAL_ADC_PollForConversion(&hadc1, 1);
+ PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
-// TIM8->CCR1 = (TMR8_COUNT)*(0.95f);
-// TIM8->CCR2 = (TMR8_COUNT)*(0.975f);
+ HAL_ADC_Start(&hadc1);
+ HAL_ADC_PollForConversion(&hadc1, 1);
+ PSEN2 = (float) HAL_ADC_GetValue(&hadc1);
+ float pres_A_new, pres_B_new;
+ pres_A_new = (((float)PSEN1) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+ pres_B_new = (((float)PSEN2) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
-// PWM_H2 = 0;
-// PWM_L2 = 1;
+ pres_A.UpdateSen(pres_A_new,FREQ_TMR3,200.0f);
+ pres_B.UpdateSen(pres_B_new,FREQ_TMR3,200.0f);
-// TIM3->CCR2 = (TMR3_COUNT)*(0.95f);
-// TIM3->CCR1 = (TMR3_COUNT)*(0.975f);
-
-
+ if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+ float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
+ torq.UpdateSen(torq_new,FREQ_TMR3,1000.0f); // unit : Nm
+ } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+ float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
+ force.UpdateSen(force_new,FREQ_TMR3,1000.0f); // unit : N
+ }
+ }
+ CNT_TMR3++;
}
TIM3->SR = 0x0; // reset the status register
}
@@ -515,8 +535,8 @@
//------------------------------------------------
-// TMR4 : Sensor Read & Data Handling
-//-----------------------------------------------
+// TMR4 : LVDT 1kHz
+//------------------------------------------------
float FREQ_TMR4 = (float)FREQ_20k;
float DT_TMR4 = (float)DT_20k;
long CNT_TMR4 = 0;
@@ -525,123 +545,59 @@
{
if (TIM4->SR & TIM_SR_UIF ) {
- // Current ===================================================
- //ADC3->CR2 |= 0x40000000; // adc _ 12bit
+ float LVDT_OUT = 0.0f;
+ LVDT_sum = 0.0f;
+
+ LVDT_L = 0;
+ LVDT_H = 1;
- cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA
+ for (int ij = 0; ij<150; ij++) {
- // Encoder ===================================================
- if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
- ENC_UPDATE();
+ ADC3->CR2 |= 0x40000000;
+ LVDT_new = ((float)ADC3->DR) - 2047.5f;
+ LVDT_sum = LVDT_sum + LVDT_new;
}
- /*
- // 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;
- // }
- pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
- pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
+ LVDT_H = 0;
+ LVDT_L = 1;
- if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
- float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
- torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f); // unit : Nm
- } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
- float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
- force.UpdateSen(force_new,FREQ_TMR4,1000.0f); // unit : N
- }
- }
- */
+ LVDT_new = LVDT_sum * 0.01f*2.0f;
-// //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;
-// }
-
-
-
-
+ 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;
CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
}
-
-int j =0;
-float FREQ_TMR5 = (float)FREQ_5k;
-float DT_TMR5 = (float)DT_5k;
+//------------------------------------------------
+// TMR2 : Control 5kHz
+//------------------------------------------------
+float FREQ_TMR2 = (float)FREQ_5k;
+float DT_TMR2 = (float)DT_5k;
int cnt_trans = 0;
-int can_rest =0;
-float force_ref_act_can = 0.0f;
-
extern "C" void TIM2_IRQHandler(void)
{
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_TMR5 /3.0f))/2.0f;
+ alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
cnt_trans++;
torq.err_int = 0.0f;
force.err_int = 0.0f;
- if((float)cnt_trans * DT_TMR5 > 3.0f)
+ if((float)cnt_trans * DT_TMR2 > 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_TMR5 /3.0f))/2.0f;
+ alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
cnt_trans++;
torq.err_int = 0.0f;
force.err_int = 0.0f;
- if((float) cnt_trans * DT_TMR5 > 3.0f )
+ if((float) cnt_trans * DT_TMR2 > 3.0f )
MODE_POS_FT_TRANS = 0;
} else if(MODE_POS_FT_TRANS == 2) {
alpha_trans = 1.0f;
@@ -1131,7 +1087,7 @@
VALVE_POS_CONTROL(valve_pos_raw.ref);
ref_array[cnt_step_test] = valve_pos_ref;
- if(valve_pos.sen >= (float) 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*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
@@ -1156,7 +1112,7 @@
}
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);
+ float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2);
if(valve_pos_ref >= 0) {
valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
} else {
@@ -1167,7 +1123,7 @@
ref_array[cnt_freq_test] = valve_pos_ref;
// if(value>=(float) VALVE_ELECTRIC_CENTER) {
- if(valve_pos.sen>=(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 {
@@ -1177,7 +1133,7 @@
CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
cnt_freq_test++;
- if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5 > 2) {
+ if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2 > 2) {
buffer_data_size = cnt_freq_test;
cnt_freq_test = 0;
cnt_send_buffer = 0;
@@ -1259,7 +1215,6 @@
// 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_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
@@ -1296,9 +1251,6 @@
}
}
-
-
-
if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
float I_MAX = 10.0f; // Maximum Current : 10mA
@@ -1464,31 +1416,29 @@
else if (PWM_out < -1.0f) PWM_out=-1.0f;
if (PWM_out>0.0f) {
- dtc_v=0.0f;
- dtc_w=PWM_out;
+ TIM1->CCR1 = (TMR1_COUNT)*(PWM_out);
+ TIM1->CCR2 = (TMR1_COUNT)*(0.0f);
} else {
- dtc_v=-PWM_out;
- dtc_w=0.0f;
+ TIM1->CCR1 = (TMR1_COUNT)*(0.0f);
+ TIM1->CCR2 = 0.0f -(TMR1_COUNT)*(PWM_out);
}
-
////////////////////////////////////////////////////////////////////////////
////////////////////// 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/500) == 0) {
-
// Position, Velocity, and Torque (ID:1200)
- 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));
-
- } 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));
- }
+ if (flag_data_request[0] == LOW) {
+ CAN_TX_POSITION_FT((int16_t) (cur.sen*1000.0f), (int16_t) (V_EXI), (int16_t) (valve_pos.sen));
+// 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));
+// }
}
// Valve Position (ID:1300)
@@ -1508,7 +1458,6 @@
TMR2_COUNT_CAN_TX = 0;
}
TMR2_COUNT_CAN_TX++;
-
}
TIM2->SR = 0x0; // reset the status register
}