![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- Revision:
- 245:e9c5ec04e378
- Parent:
- 244:30896263bd8b
- Child:
- 246: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 }