Seunghoon Shin
/
HydraulicControlBoard_Start
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 226:82a3ca333004
- Parent:
- 225:9c0becc196ba
- Child:
- 227:699c3e572283
--- a/main.cpp Thu Feb 04 05:43:47 2021 +0000
+++ b/main.cpp Wed Mar 03 10:56:25 2021 +0000
@@ -90,6 +90,8 @@
extern int CID_TX_VALVE_POSITION;
extern int CID_TX_SOMETHING;
+float pres_A_new = 0.0f;
+float pres_B_new = 0.0f;
/*******************************************************************************
* REFERENCE MODE
@@ -196,27 +198,39 @@
/*********************************
*** 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();
+// 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
+ enc_cs = 1;
eeprom.format(8,3);
eeprom.frequency(5000000); //5M
enc.format(8,0);
enc.frequency(5000000); //5M
+ enc_cs = 0;
make_delay();
+
+ // spi _ enc
+ spi_enc_set_init();
+ make_delay();
+
+ ////// bno rom
+// spi_eeprom_write(RID_BNO, (int16_t)1);
+// make_delay();
+ ////////
+
// rom
ROM_CALL_DATA();
make_delay();
@@ -230,34 +244,41 @@
TIM4->CR1 ^= TIM_CR1_UDIS;
make_delay();
- // TMR3 init
- Init_TMR3();
- TIM3->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);
+
+ // TMR3 init
+ Init_TMR3();
+ TIM3->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
+
+
+
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
NVIC_SetPriority(TIM4_IRQn, 3);
- //can.reset();
- can.filter(msg.id, 0xFFFFF000, CANStandard);
+
- // spi _ enc
- spi_enc_set_init();
- make_delay();
+
//DAC init
if (SENSING_MODE == 0) {
dac_1 = TORQUE_VREF / 3.3f;
dac_2 = 0.0f;
} else if (SENSING_MODE == 1) {
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
+ 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();
@@ -281,7 +302,7 @@
// timer_while ++;
//i2c for SW valve
- if(OPERATING_MODE == 5){
+ if(OPERATING_MODE == 5) {
read_field(i2c_slave_addr1);
if(DIR_VALVE_ENC < 0) value = 1023 - value;
}
@@ -408,7 +429,6 @@
extern "C" void TIM4_IRQHandler(void)
{
if (TIM4->SR & TIM_SR_UIF ) {
-
/*******************************************************
*** Sensor Read & Data Handling
********************************************************/
@@ -426,10 +446,16 @@
pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
- // Pressure Sensing (0~210)bar =============================================
+ // Pressure Sensing (0~210)bar =============================================
} else if (SENSING_MODE == 1) {
- float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
- float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+
+ if (DIR_VALVE_ENC > 0) {
+ pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
+ pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+ } else {
+ pres_A_new = (((float)ADC2->DR) - PRES_A_NULL);
+ pres_B_new = (((float)ADC1->DR) - PRES_B_NULL);
+ }
double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz
pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
@@ -466,6 +492,7 @@
{
if (TIM3->SR & TIM_SR_UIF ) {
+
if (((OPERATING_MODE&0b110)>>1) == 0) {
K_v = 0.4f; // Moog (LPM >> mA) , 100bar
mV_PER_mA = 500.0f; // 5000mV/10mA
@@ -548,83 +575,6 @@
break;
}
-// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
-// if (TMR3_COUNT_DEADZONE == 0) {
-// if (pos_plus_end == pos_minus_end) need_enc_init = true;
-// else temp_time = 0;
-// }
-// if (need_enc_init) {
-// if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-// pos_plus_end = pos.sen;
-// } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
-// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-// pos_minus_end = pos.sen;
-// } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
-// temp_time = TMR_FREQ_5k;
-// }
-//
-// if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
-// V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
-// VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
-//
-// } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
-// V_out = 0;
-// CUR_VELOCITY_sum += CUR_VELOCITY;
-// } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
-// if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
-// else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
-// else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
-// else DZ_temp_cnt2 = DZ_end;
-// CUR_VELOCITY_sum = 0;
-// } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
-// if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
-//
-// // Position of Dead Zone
-// // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0)
-// // | / | / |/
-// // | ______/ ___|___/ ______/|
-// // |/ / | / |
-// // /| / | / |
-// // 0V 0V 0V
-//
-// if (DZ_temp_cnt2 < DZ_end) {
-// if (TMR3_COUNT_DEADZONE % 20 != 0) {
-// CUR_VELOCITY_sum += CUR_VELOCITY;
-// } else {
-// V_out -= DZ_dir;
-// if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
-// CUR_VELOCITY_sum = 0;
-// }
-// if (DZ_temp_cnt == 5) {
-// if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
-// else VALVE_DEADZONE_PLUS = (int16_t) V_out;
-// DZ_dir = -DZ_dir;
-// DZ_temp_cnt = 0;
-// DZ_temp_cnt2++;
-// }
-// } else {
-// TMR3_COUNT_DEADZONE = -1;
-// VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
-// if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
-// VALVE_DEADZONE_PLUS = VALVE_CENTER;
-// VALVE_DEADZONE_MINUS = VALVE_CENTER;
-// }
-// V_out = 0;
-//
-//
-//
-// //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
-// //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
-//
-// CONTROL_MODE = MODE_NO_ACT;
-// DZ_temp_cnt2 = 0;
-// }
-// }
-// TMR3_COUNT_DEADZONE++;
-// break;
-// }
-
case MODE_FIND_HOME: {
if (FINDHOME_STAGE == FINDHOME_INIT) {
cnt_findhome = 0;
@@ -648,15 +598,19 @@
}
if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
- if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
- else pos.ref = pos.ref - 12.0f;
+ if (HOMEPOS_OFFSET > 0) {
+ pos.ref = pos.ref + 12.0f;
+ } else {
+ pos.ref = pos.ref - 12.0f;
+ }
+
CONTROL_MODE = MODE_JOINT_CONTROL;
alpha_trans = 0.0f;
} else {
- ENC_SET(HOMEPOS_OFFSET);
- INIT_REF_POS = HOMEPOS_OFFSET;
+ ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10));
+ INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10);
REF_POSITION = 0;
REF_VELOCITY = 0;
FINDHOME_POSITION = 0;
@@ -683,6 +637,7 @@
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]
+ CONTROL_MODE = MODE_JOINT_CONTROL;
if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
@@ -785,7 +740,7 @@
// }
// if (fl_temp_cnt2 == 100) {
//
-//
+//
//
// //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
// cur_vel_sum = 0;
@@ -829,8 +784,13 @@
if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
+ 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;
+ }
}
} else {
CONTROL_UTILITY_MODE = MODE_NO_ACT;
@@ -843,8 +803,13 @@
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));
- dac_1 = PRES_A_VREF / 3.3f;
- dac_2 = PRES_B_VREF / 3.3f;
+ 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;
+ }
}
TMR3_COUNT_PRES_NULL++;
break;
@@ -874,7 +839,7 @@
// CUR_PRES_A_mean = 0;
// CUR_PRES_B_mean = 0;
//
-//
+//
//
// //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
// //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
@@ -1156,7 +1121,7 @@
first_check = 0;
VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-
+
spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
@@ -1359,7 +1324,7 @@
} else if (CURRENT_CONTROL_MODE == 0) { //PWM
V_out = valve_pos.ref;
} else {
- I_REF = valve_pos.ref * 0.001f;
+ I_REF = (double)(valve_pos.ref * (float)mA_PER_pulse);
}
break;
}
@@ -1573,7 +1538,14 @@
double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
- I_ERR = I_REF_fil - cur.sen;
+ if (FLAG_VALVE_DEADZONE) {
+ if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA
+ else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA
+ else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA
+ } else {
+ I_REF_fil_DZ = I_REF_fil;
+ }
+ I_ERR = I_REF_fil_DZ - (double)cur.sen;
I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
@@ -1596,10 +1568,10 @@
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 - I_REF_fil_old;
- I_REF_fil_old = I_REF_fil;
+ I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
+ I_REF_fil_old = I_REF_fil_DZ;
// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
- VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV
+ VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV
double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
double Ka = 3.0f / KP_I;
@@ -1623,19 +1595,9 @@
////////////////////////////////////////////////////////////////////////////
///////////////// Dead Zone Cancellation & Linearization //////////////////
////////////////////////////////////////////////////////////////////////////
- // Dead Zone Cancellation (Mechanical Valve dead-zone)
- if (FLAG_VALVE_DEADZONE) {
- if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
- else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
-
- VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
-
- } else {
- VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
- }
// Output Voltage Linearization
- double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
+ double CUR_PWM_nonlin = (double)VALVE_PWM_RAW; // Unit : mV
double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000
// Dead Zone Cancellation (Electrical dead-zone)
@@ -1688,65 +1650,64 @@
TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
-
- if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
- // Position, Velocity, and Torque (ID:1200)
- if (flag_data_request[0] == HIGH) {
- if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
- if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
- } else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
- } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
- if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
- } else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
- }
-
- }
+ if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+ // Position, Velocity, and Torque (ID:1200)
+ if (flag_data_request[0] == HIGH) {
+ if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+ if (SENSING_MODE == 0) {
+ CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+ } else if (SENSING_MODE == 1) {
+ CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+ }
+ } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+ if (SENSING_MODE == 0) {
+ CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
+ } else if (SENSING_MODE == 1) {
+ CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+ }
+ }
-
+ }
+
+
// ID:1300
if (flag_data_request[1] == HIGH) {
CAN_TX_TORQUE((int16_t) 7); //1300
}
- // Reference Current, Current Current (ID:1700)
+ // Reference Current, Current Current (ID:1700)
if (flag_data_request[2] == HIGH) {
-
- CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0));
+
+ CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (HOMEPOS_OFFSET), (int16_t) (0));
//double t_value = 0.0f;
- // if(value>=(float) VALVE_CENTER) {
- // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- // } else {
- // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- // }
- // double t_value_ref = 0.0f;
- // if(valve_pos.ref>=(float) VALVE_CENTER) {
- // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- // } else {
- // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- // }
- //
- //
- // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+ // if(value>=(float) VALVE_CENTER) {
+ // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+ // } else {
+ // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+ // }
+ // double t_value_ref = 0.0f;
+ // if(valve_pos.ref>=(float) VALVE_CENTER) {
+ // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+ // } else {
+ // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+ // }
+ //
+ //
+ // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
}
//If it doesn't rest, below can can not work.
for (can_rest = 0; can_rest < 10000; can_rest++) {
;
}
-
+
// ID:1500
if (flag_data_request[3] == HIGH) {
//PWM
CAN_TX_PWM((int16_t) (torq.ref)); //1500
}
-
+
// ID:1600
if (flag_data_request[4] == HIGH) {
//valve position