Sungwoo Kim
LIGHT2
Diff: CAN/function_CAN.cpp
- Revision:
- 226:82a3ca333004
- Parent:
- 225:9c0becc196ba
- Child:
- 227:699c3e572283
diff -r 9c0becc196ba -r 82a3ca333004 CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp Thu Feb 04 05:43:47 2021 +0000
+++ b/CAN/function_CAN.cpp Wed Mar 03 10:56:25 2021 +0000
@@ -126,6 +126,7 @@
CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
if (CONTROL_MODE == 22) { //MODE_FIND_HOME
FLAG_FIND_HOME = true;
+ FINDHOME_STAGE = FINDHOME_INIT;
}
break;
}
@@ -290,12 +291,12 @@
case CRX_SET_VALVE_DEADZONE: {
VALVE_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
- VALVE_DEADZONE_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
- VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
+ VALVE_DEADZONE_PLUS = (int16_t)(msg.data[3] | msg.data[4] << 8);
+ VALVE_DEADZONE_MINUS = (int16_t)(msg.data[5] | msg.data[6] << 8);
- spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
- spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
- spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
+ spi_eeprom_write(RID_VALVE_CNETER, VALVE_CENTER);
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
+ spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
break;
}
@@ -545,10 +546,6 @@
break;
}
- case CRX_ASK_DDV_VALVE_DEADZONE: {
- CAN_TX_DDV_VALVE_DEADZONE();
- break;
- }
case CRX_LOW_REF: {
REFERENCE_MODE = msg.data[1];
@@ -586,7 +583,7 @@
break;
}
case CRX_SET_HOMEPOS_OFFSET: {
- HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ HOMEPOS_OFFSET = (int16_t)(msg.data[1] | msg.data[2] << 8);
spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
break;
}
@@ -619,86 +616,6 @@
case CRX_SET_ROM: {
break;
}
- case CRX_SET_VALVE_CENTER_OFFSET: {
- VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
- VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
- spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-
- break;
- }
- case CRX_SET_VALVE_DZ_MINUS_OFFSET: {
- VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
- VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET;
- spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
-
- break;
- }
- case CRX_SET_VALVE_DZ_PLUS_OFFSET: {
- VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
- VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET;
- spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
- break;
- }
- case CRX_SET_PID_GAIN_OPP: {
- if (msg.data[1] == 0) {
- P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
- I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
- D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
- //spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
- //spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
- //spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
- } else if (msg.data[1] == 1) {
- P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
- I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
- D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
- //spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
- //spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
- //spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
- } else if (msg.data[1] == 2) {
- P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
- I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
- D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
- //spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
- //spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
- //spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
- }
- break;
- }
- case CRX_ASK_VALVE_MAX_MIN_POS: {
- CAN_TX_DDV_VALVE_MAX_MIN_POS();
- break;
- }
- case CRX_DELAY_TEST: {
- flag_delay_test = 1;
- break;
- }
- case CRX_SET_NN_CONTROL_FLAG: {
- NN_Control_Flag = (int16_t) msg.data[1];
- CONTROL_UTILITY_MODE = 1;
- break;
- }
- case CRX_SET_FREQ_TEST: {
- cnt_freq_test = 0;
- CONTROL_UTILITY_MODE = 34;
- break;
- }
-
- case CRX_ASK_BUFFER: {
- cnt_buffer = (int16_t) (msg.data[1] | msg.data[2] << 8);
- CAN_TX_BUFFER(cnt_buffer);
- break;
- }
-
- case CRX_SET_STEP_TEST: {
- cnt_step_test = 0;
- CONTROL_UTILITY_MODE = 37;
- break;
- }
-
- case CRX_SET_CHANGE_EVERY_REFERNCE: {
- flag_every_reference = 1;
- break;
- }
default:
break;
@@ -721,8 +638,8 @@
int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
- pos.ref = (double)temp_pos * 1.0f;
- vel.ref = (double)temp_vel * 10.0f;
+ pos.ref = (double)temp_pos * 10.0f;
+ vel.ref = (double)temp_vel * 256.0f;
} else { //Linear Actuator
pos.ref = (double)temp_pos * 10.0f;
vel.ref = (double)temp_vel * 256.0f;
@@ -733,35 +650,35 @@
torq_dot.sen = torq.sen-torq_sen_past;
torq.ref_old = torq.ref;
torq_sen_past = torq.sen;
-
- if(CAN_FREQ == -1) {
- // 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(CAN_FREQ == -1) {
+ // 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/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen*10.0f));
+ } 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)*10.0f), (int16_t) ((pres_B.sen)*10.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)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+ }
}
}
- }
-
- // ID:1300
- if (flag_data_request[1] == HIGH) {
- CAN_TX_TORQUE((int16_t) 7); //1300
- }
-
- // 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));
- //double t_value = 0.0f;
+
+ // ID:1300
+ if (flag_data_request[1] == HIGH) {
+ CAN_TX_TORQUE((int16_t) (pos.ref/10.0f)); //1300
+ }
+
+ // Reference Current, Current Current (ID:1700)
+ if (flag_data_request[2] == HIGH) {
+
+ 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) (I_ERR / mA_PER_pulse));
+ //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 {
@@ -773,29 +690,29 @@
// } 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 (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) {
- ;
+ }
+
+ //If it doesn't rest, below can can not work.
+ for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) {
+ ;
+ }
+
+ // 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
+ CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+ }
+
}
-
- // 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
- CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
- }
-
- }
} else if(address==CID_RX_REF_VALVE_POS) {
@@ -829,7 +746,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
temp_msg.data[1] = (uint8_t) BNO;
temp_msg.data[2] = (uint8_t) CAN_FREQ;
@@ -847,7 +764,7 @@
temp_msg.id = CID_TX_INFO;
//temp_msg.len = 2;
- temp_msg.len = 8;
+ temp_msg.len = 2;
temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
temp_msg.data[1] = (uint8_t) BNO;
@@ -859,7 +776,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE;
temp_msg.data[1] = (uint8_t) OPERATING_MODE;
temp_msg.data[2] = (uint8_t) SENSING_MODE;
@@ -874,7 +791,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ;
temp_msg.data[1] = (uint8_t) CAN_FREQ;
temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8);
@@ -887,7 +804,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 2;
temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
@@ -899,7 +816,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_JOINT_ENC_DIR;
temp_msg.data[1] = (uint8_t) DIR_JOINT_ENC;
temp_msg.data[2] = (uint8_t) (DIR_JOINT_ENC >> 8);
@@ -912,7 +829,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DIR;
temp_msg.data[1] = (uint8_t) DIR_VALVE;
temp_msg.data[2] = (uint8_t) (DIR_VALVE >> 8);
@@ -925,7 +842,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ENC_DIR;
temp_msg.data[1] = (uint8_t) DIR_VALVE_ENC;
temp_msg.data[2] = (uint8_t) (DIR_VALVE_ENC >> 8);
@@ -940,7 +857,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_SUPPLY;
temp_msg.data[1] = (uint8_t) (send_voltage_supply);
temp_msg.data[2] = (uint8_t) (send_voltage_supply >> 8);
@@ -955,7 +872,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_VALVE;
temp_msg.data[1] = (uint8_t) send_voltage_valve;
temp_msg.data[2] = (uint8_t) (send_voltage_valve >> 8);
@@ -1010,14 +927,14 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
temp_msg.data[1] = (uint8_t) VALVE_CENTER;
temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
- temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS);
- temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
- temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS);
- temp_msg.data[6] = (uint8_t) ((int) (VALVE_DEADZONE_MINUS) >> 8);
+ temp_msg.data[3] = (uint8_t) VALVE_DEADZONE_PLUS;
+ temp_msg.data[4] = (uint8_t) (VALVE_DEADZONE_PLUS >> 8);
+ temp_msg.data[5] = (uint8_t) VALVE_DEADZONE_MINUS;
+ temp_msg.data[6] = (uint8_t) (VALVE_DEADZONE_MINUS >> 8);
can.write(temp_msg);
}
@@ -1027,7 +944,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VELOCITY_COMP_GAIN;
temp_msg.data[1] = (uint8_t) VELOCITY_COMP_GAIN;
temp_msg.data[2] = (uint8_t) (VELOCITY_COMP_GAIN >> 8);
@@ -1040,7 +957,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_COMPLIANCE_GAIN;
temp_msg.data[1] = (uint8_t) COMPLIANCE_GAIN;
temp_msg.data[2] = (uint8_t) (COMPLIANCE_GAIN >> 8);
@@ -1053,7 +970,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_FF;
temp_msg.data[1] = (uint8_t) VALVE_FF;
temp_msg.data[2] = (uint8_t) (VALVE_FF >> 8);
@@ -1066,7 +983,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_BULK_MODULUS;
temp_msg.data[1] = (uint8_t) BULK_MODULUS;
temp_msg.data[2] = (uint8_t) (BULK_MODULUS >> 8);
@@ -1079,7 +996,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_CHAMBER_VOLUME;
temp_msg.data[1] = (uint8_t) CHAMBER_VOLUME_A;
temp_msg.data[2] = (uint8_t) (CHAMBER_VOLUME_A >> 8);
@@ -1094,7 +1011,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_PISTON_AREA;
temp_msg.data[1] = (uint8_t) PISTON_AREA_A;
temp_msg.data[2] = (uint8_t) (PISTON_AREA_A >> 8);
@@ -1109,7 +1026,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_PRES;
temp_msg.data[1] = (uint8_t) PRES_SUPPLY;
temp_msg.data[2] = (uint8_t) (PRES_SUPPLY >> 8);
@@ -1124,7 +1041,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_LIMIT;
temp_msg.data[1] = (uint8_t) ENC_LIMIT_MINUS;
temp_msg.data[2] = (uint8_t) (ENC_LIMIT_MINUS >> 8);
@@ -1139,7 +1056,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
temp_msg.data[1] = (uint8_t) STROKE;
temp_msg.data[2] = (uint8_t) (STROKE >> 8);
@@ -1152,7 +1069,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
@@ -1167,7 +1084,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION);
temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position;
@@ -1181,7 +1098,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f);
temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
@@ -1195,7 +1112,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
temp_msg.data[1] = (uint8_t) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f);
temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f) >> 8);
@@ -1212,7 +1129,7 @@
send_friction = (int16_t) (FRICTION * 10.0f);
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_FRICTION;
temp_msg.data[1] = (uint8_t) send_friction;
temp_msg.data[2] = (uint8_t) (send_friction >> 8);
@@ -1225,7 +1142,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS;
temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[0] * 50.0f);
temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[2] * 50.0f);
@@ -1236,36 +1153,13 @@
can.write(temp_msg);
}
-void CAN_TX_DDV_VALVE_DEADZONE(void)
-{
- CANMessage temp_msg;
- float temp_valve_deadzone_minus = 0.0f;
- float temp_valve_deadzone_plus = 0.0f;
- float temp_ddv_center = 0.0f;
-
- temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS;
- temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS;
- temp_ddv_center = (double)VALVE_CENTER;
-
- temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
- temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE;
- temp_msg.data[1] = (uint8_t) temp_valve_deadzone_minus;
- temp_msg.data[2] = (uint8_t) ((int) (temp_valve_deadzone_minus) >> 8);
- temp_msg.data[3] = (uint8_t) (temp_valve_deadzone_plus);
- temp_msg.data[4] = (uint8_t) ((int) (temp_valve_deadzone_plus) >> 8);
- temp_msg.data[5] = (uint8_t) (temp_ddv_center);
- temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
-
- can.write(temp_msg);
-}
void CAN_TX_VALVE_GAIN_MINUS(void)
{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS;
temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[1] * 50.0f);
temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[3] * 50.0f);
@@ -1281,7 +1175,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) CTX_SEND_REFENCE_MODE;
temp_msg.data[1] = (uint8_t) REFERENCE_MODE;
temp_msg.data[2] = (uint8_t) (int) (REFERENCE_FREQ * 100.0f);
@@ -1296,10 +1190,10 @@
{
CANMessage temp_msg;
int16_t send_homepos_offset;
- send_homepos_offset = (int16_t) (HOMEPOS_OFFSET);
+ send_homepos_offset = (int16_t)HOMEPOS_OFFSET;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_OFFSET;
temp_msg.data[1] = (uint8_t) send_homepos_offset;
temp_msg.data[2] = (uint8_t) (send_homepos_offset >> 8);
@@ -1314,7 +1208,7 @@
send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING);
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_VALVE_OPENING;
temp_msg.data[1] = (uint8_t) send_homepos_valve_opening;
temp_msg.data[2] = (uint8_t) (send_homepos_valve_opening >> 8);
@@ -1337,7 +1231,7 @@
PWM_VALVE_ID = ID_index_array[canindex] * 1000;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_VALVE_PWM_VS_VALVE_POS;
temp_msg.data[1] = (uint8_t) PWM_VALVE_ID;
temp_msg.data[2] = (uint8_t) (PWM_VALVE_ID >> 8);
@@ -1362,7 +1256,7 @@
}
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
temp_msg.data[1] = (uint8_t) temp_valve_pos;
temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8);
@@ -1381,7 +1275,7 @@
valve_pos_num = (int16_t) VALVE_POS_NUM;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_NUM;
temp_msg.data[1] = (uint8_t) valve_pos_num;
temp_msg.data[2] = (uint8_t) (valve_pos_num >> 8);
@@ -1402,7 +1296,7 @@
temp_ddv_center = VALVE_CENTER;
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
temp_msg.data[1] = (uint8_t) temp_valve_max_pos;
temp_msg.data[2] = (uint8_t) ((int) (temp_valve_max_pos) >> 8);
@@ -1423,7 +1317,7 @@
send_ref_array = (int16_t) (ref_array[t_cnt_buffer]);
temp_msg.id = CID_TX_INFO;
- temp_msg.len = 8;
+ temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_BUFFER;
temp_msg.data[1] = (uint8_t) send_pos_array;
temp_msg.data[2] = (uint8_t) (send_pos_array >> 8);
@@ -1443,7 +1337,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_POSITION;
- temp_msg.len = 8;
+ temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) t_pos;
temp_msg.data[1] = (uint8_t) (t_pos >> 8);
temp_msg.data[2] = (uint8_t) t_vel;
@@ -1474,14 +1368,14 @@
}
-void CAN_TX_TORQUE(int16_t t_valve_pos)
+void CAN_TX_TORQUE(int16_t t_torque)
{
CANMessage temp_msg;
temp_msg.id = CID_TX_TORQUE;
- temp_msg.len = 8;
- temp_msg.data[0] = (uint8_t) t_valve_pos;
- temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
+ temp_msg.len = 2;
+ temp_msg.data[0] = (uint8_t) t_torque;
+ temp_msg.data[1] = (uint8_t) (t_torque >> 8);
can.write(temp_msg);
}
@@ -1491,7 +1385,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_PRES;
- temp_msg.len = 8;
+ temp_msg.len = 4;
temp_msg.data[0] = (uint8_t) t_pres_a;
temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
temp_msg.data[2] = (uint8_t) t_pres_b;
@@ -1505,7 +1399,7 @@
CANMessage temp_msg;
temp_msg.id = CID_TX_VOUT;
- temp_msg.len = 8;
+ temp_msg.len = 2;
temp_msg.data[0] = (uint8_t) t_pwm;
temp_msg.data[1] = (uint8_t) (t_pwm >> 8);