Sungwoo Kim
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 12:6f2531038ea4
- Parent:
- 11:82d8768d7351
- Child:
- 14:8e7590227d22
diff -r 82d8768d7351 -r 6f2531038ea4 CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp Fri Aug 23 12:28:08 2019 +0000
+++ b/CAN/function_CAN.cpp Tue Aug 27 05:32:44 2019 +0000
@@ -18,6 +18,9 @@
int CID_TX_VOUT = 1500;
int CID_TX_VALVE_POSITION = 1600;
+// variables
+uint8_t can_index = 0;
+
/*******************************************************************************
* CAN functions
@@ -39,7 +42,7 @@
CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION);
}
-void ReadCMD(char CMD)
+void ReadCMD(int8_t CMD)
{
switch(CMD){
case CRX_ASK_INFO: {
@@ -71,7 +74,7 @@
break;
}
case CRX_SET_FET_ON: {
- LAT_MOT_EN = (int16_t) msg.data[1];;
+ //LAT_MOT_EN = (int16_t) msg.data[1];;
break;
}
@@ -102,7 +105,7 @@
case CRX_SET_CONTROL_MODE: {
CONTROL_MODE = (int16_t) (msg.data[1]);
- if (CONTROL_MODE == MODE_FIND_HOME) {
+ if (CONTROL_MODE == 22) { //MODE_FIND_HOME
FLAG_FIND_HOME = true;
}
break;
@@ -112,8 +115,10 @@
int request_type = msg.data[2];
flag_data_request[request_type] = msg.data[1];
- if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
- if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
+// if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
+ if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3;
+// if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
+ if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
break;
}
@@ -196,7 +201,7 @@
break;
}
- ///////////////////////////////////////////////////////////////////////////////////////////////////////////
+
case CRX_SET_HOMEPOS: {
CONTROL_MODE = 22;
SETTING_SWITCH = (int16_t) (msg.data[1]);
@@ -236,8 +241,6 @@
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;
@@ -351,7 +354,10 @@
case CRX_ASK_PRES: {
CAN_TX_PRES_A_AND_B();
- SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
+// SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
+ dac_1 = PRES_A_VREF;
+ dac_2 = PRES_B_VREF;
+
break;
}
@@ -535,7 +541,7 @@
case CRX_SET_ERR_CLEAR: {
- for (i = 0; i < num_err; i++) {
+ for (int i = 0; i < num_err; i++) {
flag_err[i] = FALSE;
flag_err_old[i] = FALSE;
}
@@ -621,28 +627,28 @@
*******************************************************************************/
void CAN_TX_INFO(void) {
- CANmsg.data temp_msg;
+ CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
- temp_msg.data[0] = (unsigned char) CTX_SEND_INFO;
- temp_msg.data[1] = (unsigned char) BNO;
- temp_msg.data[2] = (unsigned char) CAN_FREQ;
- temp_msg.data[3] = (unsigned char) (CAN_FREQ >> 8);
- temp_msg.data[4] = (unsigned char) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
- temp_msg.data[5] = (unsigned char) CONTROL_MODE;
- temp_msg.data[6] = (unsigned char) OPERATING_MODE;
+ temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
+ temp_msg.data[1] = (uint8_t) BNO;
+ temp_msg.data[2] = (uint8_t) CAN_FREQ;
+ temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8);
+ temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
+ temp_msg.data[5] = (uint8_t) CONTROL_MODE;
+ temp_msg.data[6] = (uint8_t) OPERATING_MODE;
can.write(temp_msg);
}
void CAN_TX_BNO(void) {
- CANmsg.data temp_msg;
+ CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
- temp_msg.data[0] = (unsigned char) CTX_SEND_BNO;
- temp_msg.data[1] = (unsigned char) BNO;
+ temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
+ temp_msg.data[1] = (uint8_t) BNO;
can.write(temp_msg);
}
@@ -652,8 +658,8 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
- temp_msg.data[0] = (unsigned char) CTX_SEND_OPERATING_MODE;
- temp_msg.data[1] = (unsigned char) OPERATING_MODE;
+ temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE;
+ temp_msg.data[1] = (uint8_t) OPERATING_MODE;
can.write(temp_msg);
}
@@ -675,8 +681,8 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
- temp_msg.data[0] = (unsigned char) CTX_SEND_CONTROL_MODE;
- temp_msg.data[1] = (unsigned char) CONTROL_MODE;
+ temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
+ temp_msg.data[1] = (uint8_t) CONTROL_MODE;
can.write(temp_msg);
}
@@ -686,9 +692,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_JOINT_ENC_DIR;
- temp_msg.data[1] = (unsigned char) DIR_JOINT_ENC;
- temp_msg.data[2] = (unsigned char) (DIR_JOINT_ENC >> 8);
+ 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);
can.write(temp_msg);
}
@@ -698,9 +704,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DIR;
- temp_msg.data[1] = (unsigned char) DIR_VALVE;
- temp_msg.data[2] = (unsigned char) (DIR_VALVE >> 8);
+ 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);
can.write(temp_msg);
}
@@ -710,37 +716,37 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_ENC_DIR;
- temp_msg.data[1] = (unsigned char) DIR_VALVE_ENC;
- temp_msg.data[2] = (unsigned char) (DIR_VALVE_ENC >> 8);
+ 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);
can.write(temp_msg);
}
void CAN_TX_VOLTAGE_SUPPLY(void) {
- long send_voltage_supply = (long) (SUPPLY_VOLTAGE * 10);
+ int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10);
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_SUPPLY;
- temp_msg.data[1] = (unsigned char) (send_voltage_supply);
- temp_msg.data[2] = (unsigned char) (send_voltage_supply >> 8);
+ 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);
can.write(temp_msg);
}
void CAN_TX_VOLTAGE_VALVE(void) {
- long send_voltage_valve = (long) (VALVE_VOLTAGE_LIMIT * 10);
+ int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10);
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_VALVE;
- temp_msg.data[1] = (unsigned char) send_voltage_valve;
- temp_msg.data[2] = (unsigned char) (send_voltage_valve >> 8);
+ 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);
can.write(temp_msg);
}
@@ -751,33 +757,33 @@
// t_type = 1 : joint position control gain
// t_type = 2 : joint torque control gain
- long sendPgain=0, sendIgain=0, sendDgain=0;
+ int16_t sendPgain=0, sendIgain=0, sendDgain=0;
if (t_type == 0) {
-// sendPgain = (long) (P_GAIN_VALVE_POSITION);
-// sendIgain = (long) (I_GAIN_VALVE_POSITION);
-// sendDgain = (long) (D_GAIN_VALVE_POSITION);
+// sendPgain = (int16_t) (P_GAIN_VALVE_POSITION);
+// sendIgain = (int16_t) (I_GAIN_VALVE_POSITION);
+// sendDgain = (int16_t) (D_GAIN_VALVE_POSITION);
} else if (t_type == 1) {
- sendPgain = (long) (P_GAIN_JOINT_POSITION);
- sendIgain = (long) (I_GAIN_JOINT_POSITION);
- sendDgain = (long) (D_GAIN_JOINT_POSITION);
+ sendPgain = (int16_t) (P_GAIN_JOINT_POSITION);
+ sendIgain = (int16_t) (I_GAIN_JOINT_POSITION);
+ sendDgain = (int16_t) (D_GAIN_JOINT_POSITION);
} else if (t_type == 2) {
- sendPgain = (long) (P_GAIN_JOINT_TORQUE);
- sendIgain = (long) (I_GAIN_JOINT_TORQUE);
- sendDgain = (long) (D_GAIN_JOINT_TORQUE);
+ sendPgain = (int16_t) (P_GAIN_JOINT_TORQUE);
+ sendIgain = (int16_t) (I_GAIN_JOINT_TORQUE);
+ sendDgain = (int16_t) (D_GAIN_JOINT_TORQUE);
}
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 8;
- temp_msg.data[0] = (unsigned char) CTX_SEND_PID_GAIN;
- temp_msg.data[1] = (unsigned char) t_type;
- temp_msg.data[2] = (unsigned char) sendPgain;
- temp_msg.data[3] = (unsigned char) (sendPgain >> 8);
- temp_msg.data[4] = (unsigned char) sendIgain;
- temp_msg.data[5] = (unsigned char) (sendIgain >> 8);
- temp_msg.data[6] = (unsigned char) sendDgain;
- temp_msg.data[7] = (unsigned char) (sendDgain >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_PID_GAIN;
+ temp_msg.data[1] = (uint8_t) t_type;
+ temp_msg.data[2] = (uint8_t) sendPgain;
+ temp_msg.data[3] = (uint8_t) (sendPgain >> 8);
+ temp_msg.data[4] = (uint8_t) sendIgain;
+ temp_msg.data[5] = (uint8_t) (sendIgain >> 8);
+ temp_msg.data[6] = (uint8_t) sendDgain;
+ temp_msg.data[7] = (uint8_t) (sendDgain >> 8);
can.write(temp_msg);
}
@@ -788,13 +794,13 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DEADZONE;
- temp_msg.data[1] = (unsigned char) (int) (VALVE_CENTER);
- temp_msg.data[2] = (unsigned char) ((int) (VALVE_CENTER) >> 8);
- temp_msg.data[3] = (unsigned char) (int) (VALVE_DEADZONE_PLUS);
- temp_msg.data[4] = (unsigned char) ((int) (VALVE_DEADZONE_PLUS) >> 8);
- temp_msg.data[5] = (unsigned char) (int) (VALVE_DEADZONE_MINUS);
- temp_msg.data[6] = (unsigned char) ((int) (VALVE_DEADZONE_MINUS) >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
+ temp_msg.data[1] = (uint8_t) (int) (VALVE_CENTER);
+ temp_msg.data[2] = (uint8_t) ((int) (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);
can.write(temp_msg);
}
@@ -804,9 +810,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VELOCITY_COMP_GAIN;
- temp_msg.data[1] = (unsigned char) VELOCITY_COMP_GAIN;
- temp_msg.data[2] = (unsigned char) (VELOCITY_COMP_GAIN >> 8);
+ 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);
can.write(temp_msg);
}
@@ -816,9 +822,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_COMPLIANCE_GAIN;
- temp_msg.data[1] = (unsigned char) COMPLIANCE_GAIN;
- temp_msg.data[2] = (unsigned char) (COMPLIANCE_GAIN >> 8);
+ 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);
can.write(temp_msg);
}
@@ -828,9 +834,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_FF;
- temp_msg.data[1] = (unsigned char) VALVE_FF;
- temp_msg.data[2] = (unsigned char) (VALVE_FF >> 8);
+ 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);
can.write(temp_msg);
}
@@ -840,9 +846,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_BULK_MODULUS;
- temp_msg.data[1] = (unsigned char) BULK_MODULUS;
- temp_msg.data[2] = (unsigned char) (BULK_MODULUS >> 8);
+ 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);
can.write(temp_msg);
}
@@ -852,11 +858,11 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_SEND_CHAMBER_VOLUME;
- temp_msg.data[1] = (unsigned char) CHAMBER_VOLUME_A;
- temp_msg.data[2] = (unsigned char) (CHAMBER_VOLUME_A >> 8);
- temp_msg.data[3] = (unsigned char) CHAMBER_VOLUME_B;
- temp_msg.data[4] = (unsigned char) (CHAMBER_VOLUME_B >> 8);
+ 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);
+ temp_msg.data[3] = (uint8_t) CHAMBER_VOLUME_B;
+ temp_msg.data[4] = (uint8_t) (CHAMBER_VOLUME_B >> 8);
can.write(temp_msg);
}
@@ -866,11 +872,11 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_SEND_PISTON_AREA;
- temp_msg.data[1] = (unsigned char) PISTON_AREA_A;
- temp_msg.data[2] = (unsigned char) (PISTON_AREA_A >> 8);
- temp_msg.data[3] = (unsigned char) PISTON_AREA_B;
- temp_msg.data[4] = (unsigned char) (PISTON_AREA_B >> 8);
+ 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);
+ temp_msg.data[3] = (uint8_t) PISTON_AREA_B;
+ temp_msg.data[4] = (uint8_t) (PISTON_AREA_B >> 8);
can.write(temp_msg);
}
@@ -880,11 +886,11 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_SEND_PRES;
- temp_msg.data[1] = (unsigned char) PRES_SUPPLY;
- temp_msg.data[2] = (unsigned char) (PRES_SUPPLY >> 8);
- temp_msg.data[3] = (unsigned char) PRES_RETURN;
- temp_msg.data[4] = (unsigned char) (PRES_RETURN >> 8);
+ 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);
+ temp_msg.data[3] = (uint8_t) PRES_RETURN;
+ temp_msg.data[4] = (uint8_t) (PRES_RETURN >> 8);
can.write(temp_msg);
}
@@ -894,11 +900,11 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_LIMIT;
- temp_msg.data[1] = (unsigned char) ENC_LIMIT_MINUS;
- temp_msg.data[2] = (unsigned char) (ENC_LIMIT_MINUS >> 8);
- temp_msg.data[3] = (unsigned char) ENC_LIMIT_PLUS;
- temp_msg.data[4] = (unsigned char) (ENC_LIMIT_PLUS >> 8);
+ 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);
+ temp_msg.data[3] = (uint8_t) ENC_LIMIT_PLUS;
+ temp_msg.data[4] = (uint8_t) (ENC_LIMIT_PLUS >> 8);
can.write(temp_msg);
}
@@ -908,9 +914,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_STROKE;
- temp_msg.data[1] = (unsigned char) STROKE;
- temp_msg.data[2] = (unsigned char) (STROKE >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
+ temp_msg.data[1] = (uint8_t) STROKE;
+ temp_msg.data[2] = (uint8_t) (STROKE >> 8);
can.write(temp_msg);
}
@@ -920,11 +926,11 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_LIMIT;
- temp_msg.data[1] = (unsigned char) VALVE_LIMIT_MINUS;
- temp_msg.data[2] = (unsigned char) (VALVE_LIMIT_MINUS >> 8);
- temp_msg.data[3] = (unsigned char) VALVE_LIMIT_PLUS;
- temp_msg.data[4] = (unsigned char) (VALVE_LIMIT_PLUS >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
+ temp_msg.data[1] = (uint8_t) VALVE_LIMIT_MINUS;
+ temp_msg.data[2] = (uint8_t) (VALVE_LIMIT_MINUS >> 8);
+ temp_msg.data[3] = (uint8_t) VALVE_LIMIT_PLUS;
+ temp_msg.data[4] = (uint8_t) (VALVE_LIMIT_PLUS >> 8);
can.write(temp_msg);
}
@@ -934,9 +940,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_PULSE_PER_POSITION;
- temp_msg.data[1] = (unsigned char) ENC_PULSE_PER_POSITION;
- temp_msg.data[2] = (unsigned char) (ENC_PULSE_PER_POSITION >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
+ temp_msg.data[1] = (uint8_t) ENC_PULSE_PER_POSITION;
+ temp_msg.data[2] = (uint8_t) (ENC_PULSE_PER_POSITION >> 8);
can.write(temp_msg);
}
@@ -946,9 +952,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
- temp_msg.data[1] = (unsigned char) TORQUE_SENSOR_PULSE_PER_TORQUE;
- temp_msg.data[2] = (unsigned char) (TORQUE_SENSOR_PULSE_PER_TORQUE >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
+ temp_msg.data[1] = (uint8_t) TORQUE_SENSOR_PULSE_PER_TORQUE;
+ temp_msg.data[2] = (uint8_t) (TORQUE_SENSOR_PULSE_PER_TORQUE >> 8);
can.write(temp_msg);
}
@@ -958,25 +964,25 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
- temp_msg.data[1] = (unsigned char) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.);
- temp_msg.data[2] = (unsigned char) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.) >> 8);
- temp_msg.data[3] = (unsigned char) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.);
- temp_msg.data[4] = (unsigned char) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.) >> 8);
+ 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.);
+ temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.) >> 8);
+ temp_msg.data[3] = (uint8_t) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.);
+ temp_msg.data[4] = (uint8_t) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.) >> 8);
can.write(temp_msg);
}
void CAN_TX_FRICTION(void) {
CANMessage temp_msg;
- long send_friction;
- send_friction = (long) (FRICTION * 10.);
+ int16_t send_friction;
+ send_friction = (int16_t) (FRICTION * 10.);
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_FRICTION;
- temp_msg.data[1] = (unsigned char) send_friction;
- temp_msg.data[2] = (unsigned char) (send_friction >> 8);
+ 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);
can.write(temp_msg);
}
@@ -986,12 +992,12 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 6;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_GAIN_PLUS;
- temp_msg.data[1] = (unsigned char) (VALVE_GAIN_LPM_PER_V[0] * 50.);
- temp_msg.data[2] = (unsigned char) (VALVE_GAIN_LPM_PER_V[2] * 50.);
- temp_msg.data[3] = (unsigned char) (VALVE_GAIN_LPM_PER_V[4] * 50.);
- temp_msg.data[4] = (unsigned char) (VALVE_GAIN_LPM_PER_V[6] * 50.);
- temp_msg.data[5] = (unsigned char) (VALVE_GAIN_LPM_PER_V[8] * 50.);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS;
+ temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[0] * 50.);
+ temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[2] * 50.);
+ temp_msg.data[3] = (uint8_t) (VALVE_GAIN_LPM_PER_V[4] * 50.);
+ temp_msg.data[4] = (uint8_t) (VALVE_GAIN_LPM_PER_V[6] * 50.);
+ temp_msg.data[5] = (uint8_t) (VALVE_GAIN_LPM_PER_V[8] * 50.);
can.write(temp_msg);
}
@@ -1001,13 +1007,13 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
- temp_msg.data[0] = (unsigned char) CTX_SEND_DDV_VALVE_DEADZONE;
- temp_msg.data[1] = (unsigned char) VALVE_DEADZONE_MINUS;
- temp_msg.data[2] = (unsigned char) ((int) (VALVE_DEADZONE_MINUS) >> 8);
- temp_msg.data[3] = (unsigned char) (VALVE_DEADZONE_PLUS);
- temp_msg.data[4] = (unsigned char) ((int) (VALVE_DEADZONE_PLUS) >> 8);
- temp_msg.data[5] = (unsigned char) (DDV_CENTER);
- temp_msg.data[6] = (unsigned char) ((int) (DDV_CENTER) >> 8);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE;
+ temp_msg.data[1] = (uint8_t) VALVE_DEADZONE_MINUS;
+ temp_msg.data[2] = (uint8_t) ((int) (VALVE_DEADZONE_MINUS) >> 8);
+ temp_msg.data[3] = (uint8_t) (VALVE_DEADZONE_PLUS);
+ temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
+ temp_msg.data[5] = (uint8_t) (DDV_CENTER);
+ temp_msg.data[6] = (uint8_t) ((int) (DDV_CENTER) >> 8);
can.write(temp_msg);
}
@@ -1017,12 +1023,12 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 6;
- temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_GAIN_MINUS;
- temp_msg.data[1] = (unsigned char) (VALVE_GAIN_LPM_PER_V[1] * 50.);
- temp_msg.data[2] = (unsigned char) (VALVE_GAIN_LPM_PER_V[3] * 50.);
- temp_msg.data[3] = (unsigned char) (VALVE_GAIN_LPM_PER_V[5] * 50.);
- temp_msg.data[4] = (unsigned char) (VALVE_GAIN_LPM_PER_V[7] * 50.);
- temp_msg.data[5] = (unsigned char) (VALVE_GAIN_LPM_PER_V[9] * 50.);
+ temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS;
+ temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[1] * 50.);
+ temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[3] * 50.);
+ temp_msg.data[3] = (uint8_t) (VALVE_GAIN_LPM_PER_V[5] * 50.);
+ temp_msg.data[4] = (uint8_t) (VALVE_GAIN_LPM_PER_V[7] * 50.);
+ temp_msg.data[5] = (uint8_t) (VALVE_GAIN_LPM_PER_V[9] * 50.);
can.write(temp_msg);
}
@@ -1032,63 +1038,64 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 6;
- temp_msg.data[0] = (unsigned char) CTX_SEND_REFENCE_MODE;
- temp_msg.data[1] = (unsigned char) REFERENCE_MODE;
- temp_msg.data[2] = (unsigned char) (int) (REFERENCE_FREQ * 100.);
- temp_msg.data[3] = (unsigned char) ((int) (REFERENCE_FREQ * 100.) >> 8);
- temp_msg.data[4] = (unsigned char) (int) (REFERENCE_MAG * 100.);
- temp_msg.data[5] = (unsigned char) ((int) (REFERENCE_MAG * 100.) >> 8);
+ 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.);
+ temp_msg.data[3] = (uint8_t) ((int) (REFERENCE_FREQ * 100.) >> 8);
+ temp_msg.data[4] = (uint8_t) (int) (REFERENCE_MAG * 100.);
+ temp_msg.data[5] = (uint8_t) ((int) (REFERENCE_MAG * 100.) >> 8);
can.write(temp_msg);
}
void CAN_TX_HOMEPOS_OFFSET(void) {
CANMessage temp_msg;
- long send_homepos_offset;
- send_homepos_offset = (long) (HOMEPOS_OFFSET);
+ int16_t send_homepos_offset;
+ send_homepos_offset = (int16_t) (HOMEPOS_OFFSET);
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_OFFSET;
- temp_msg.data[1] = (unsigned char) send_homepos_offset;
- temp_msg.data[2] = (unsigned char) (send_homepos_offset >> 8);
+ 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);
can.write(temp_msg);
}
void CAN_TX_HOMPOS_VALVE_OPENING(void) {
CANMessage temp_msg;
- long send_homepos_valve_opening;
- send_homepos_valve_opening = (long) (HOMEPOS_VALVE_OPENING);
+ int16_t send_homepos_valve_opening;
+ send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING);
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_VALVE_OPENING;
- temp_msg.data[1] = (unsigned char) send_homepos_valve_opening;
- temp_msg.data[2] = (unsigned char) (send_homepos_valve_opening >> 8);
+ 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);
can.write(temp_msg);
}
-void CAN_TX_VALVE_PWM_VS_VALVE_POS(char canindex) {
+void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex) {
CANMessage temp_msg;
int16_t valve_pos_vs_pwm;
valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
- int16_t PWM_VALVE_ID = ID_index_array[canindex] * 1000;
+ int16_t PWM_VALVE_ID;
+ PWM_VALVE_ID = ID_index_array[canindex] * 1000;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
- temp_msg.data[0] = (unsigned char) CTX_VALVE_PWM_VS_VALVE_POS;
- temp_msg.data[1] = (unsigned char) PWM_VALVE_ID;
- temp_msg.data[2] = (unsigned char) (PWM_VALVE_ID >> 8);
- temp_msg.data[3] = (unsigned char) valve_pos_vs_pwm;
- temp_msg.data[4] = (unsigned char) (valve_pos_vs_pwm >> 8);
+ 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);
+ temp_msg.data[3] = (uint8_t) valve_pos_vs_pwm;
+ temp_msg.data[4] = (uint8_t) (valve_pos_vs_pwm >> 8);
can.write(temp_msg);
}
-void CAN_TX_VALVE_POS_VS_FLOWRATE(char canindex) {
+void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex) {
CANMessage temp_msg;
int32_t valve_pos_vs_flowrate;
valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
@@ -1097,13 +1104,13 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
- temp_msg.data[0] = (unsigned char) CTX_VALVE_POS_VS_FLOWRATE;
- temp_msg.data[1] = (unsigned char) VALVE_POS_VALVE_ID;
- temp_msg.data[2] = (unsigned char) (VALVE_POS_VALVE_ID >> 8);
- temp_msg.data[3] = (unsigned char) valve_pos_vs_flowrate;
- temp_msg.data[4] = (unsigned char) (valve_pos_vs_flowrate >> 8);
- temp_msg.data[5] = (unsigned char) (valve_pos_vs_flowrate >> 16);
- temp_msg.data[6] = (unsigned char) (valve_pos_vs_flowrate >> 24);
+ temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
+ temp_msg.data[1] = (uint8_t) VALVE_POS_VALVE_ID;
+ temp_msg.data[2] = (uint8_t) (VALVE_POS_VALVE_ID >> 8);
+ temp_msg.data[3] = (uint8_t) valve_pos_vs_flowrate;
+ temp_msg.data[4] = (uint8_t) (valve_pos_vs_flowrate >> 8);
+ temp_msg.data[5] = (uint8_t) (valve_pos_vs_flowrate >> 16);
+ temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 24);
can.write(temp_msg);
}
@@ -1115,9 +1122,9 @@
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
- temp_msg.data[0] = (unsigned char) CTX_VALVE_POS_NUM;
- temp_msg.data[1] = (unsigned char) valve_pos_num;
- temp_msg.data[2] = (unsigned char) (valve_pos_num >> 8);
+ 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);
can.write(temp_msg);
}
@@ -1126,19 +1133,19 @@
Sensor & State Transmission Functions
*******************************************************************************/
-void CAN_TX_POSITION(long t_pos, long t_vel) {
+void CAN_TX_POSITION(int32_t t_pos, int32_t t_vel) {
CANMessage temp_msg;
temp_msg.id = CID_TX_POSITION;
temp_msg.len = 8;
- temp_msg.data[0] = (unsigned char) t_pos;
- temp_msg.data[1] = (unsigned char) (t_pos >> 8);
- temp_msg.data[2] = (unsigned char) (t_pos >> 16);
- temp_msg.data[3] = (unsigned char) (t_pos >> 24);
- temp_msg.data[4] = (unsigned char) t_vel;
- temp_msg.data[5] = (unsigned char) (t_vel >> 8);
- temp_msg.data[6] = (unsigned char) (t_vel >> 16);
- temp_msg.data[7] = (unsigned char) (t_vel >> 24);
+ temp_msg.data[0] = (uint8_t) t_pos;
+ temp_msg.data[1] = (uint8_t) (t_pos >> 8);
+ temp_msg.data[2] = (uint8_t) (t_pos >> 16);
+ temp_msg.data[3] = (uint8_t) (t_pos >> 24);
+ temp_msg.data[4] = (uint8_t) t_vel;
+ temp_msg.data[5] = (uint8_t) (t_vel >> 8);
+ temp_msg.data[6] = (uint8_t) (t_vel >> 16);
+ temp_msg.data[7] = (uint8_t) (t_vel >> 24);
can.write(temp_msg);
}
@@ -1148,8 +1155,8 @@
temp_msg.id = CID_TX_TORQUE;
temp_msg.len = 2;
- temp_msg.data[0] = (unsigned char) t_torque;
- temp_msg.data[1] = (unsigned char) (t_torque >> 8);
+ temp_msg.data[0] = (uint8_t) t_torque;
+ temp_msg.data[1] = (uint8_t) (t_torque >> 8);
can.write(temp_msg);
}
@@ -1159,10 +1166,10 @@
temp_msg.id = CID_TX_PRES;
temp_msg.len = 8;
- temp_msg.data[0] = (unsigned char) t_pres_a;
- temp_msg.data[1] = (unsigned char) (t_pres_a >> 8);
- temp_msg.data[2] = (unsigned char) t_pres_b;
- temp_msg.data[3] = (unsigned char) (t_pres_b >> 8);
+ 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;
+ temp_msg.data[3] = (uint8_t) (t_pres_b >> 8);
can.write(temp_msg);
}
@@ -1172,8 +1179,8 @@
temp_msg.id = CID_TX_POSITION;
temp_msg.len = 8;
- temp_msg.data[0] = (unsigned char) t_pwm;
- temp_msg.data[1] = (unsigned char) (t_pwm >> 8);
+ temp_msg.data[0] = (uint8_t) t_pwm;
+ temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
can.write(temp_msg);
}
@@ -1183,8 +1190,8 @@
temp_msg.id = CID_TX_VALVE_POSITION;
temp_msg.len = 8;
- temp_msg.data[0] = (unsigned char) t_valve_pos;
- temp_msg.data[1] = (unsigned char) (t_valve_pos >> 8);
+ temp_msg.data[0] = (uint8_t) t_valve_pos;
+ temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
can.write(temp_msg);
}