Yeseong Jeong
20210203
Diff: CAN/function_CAN.cpp
- Revision:
- 170:42c938a40313
- Parent:
- 169:645207e160ca
- Child:
- 177:8e9cf31d63f4
--- a/CAN/function_CAN.cpp Mon Nov 16 02:23:23 2020 +0000
+++ b/CAN/function_CAN.cpp Wed Nov 18 12:03:39 2020 +0000
@@ -33,7 +33,8 @@
/*******************************************************************************
* CAN functions
******************************************************************************/
-void CAN_ID_INIT(void) {
+void CAN_ID_INIT(void)
+{
CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
@@ -50,7 +51,7 @@
void ReadCMD(int16_t CMD)
{
- switch(CMD){
+ switch(CMD) {
case CRX_ASK_INFO: {
CAN_TX_INFO();
break;
@@ -61,10 +62,9 @@
}
case CRX_SET_BNO: {
BNO = (int16_t) msg.data[1];
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_BNO, (int16_t) BNO);
+
+ //ROM_RESET_DATA();
+ spi_eeprom_write(RID_BNO, (int16_t) BNO);
CAN_ID_INIT(); // can id init
break;
}
@@ -77,7 +77,11 @@
SENSING_MODE = (uint8_t) msg.data[2];
CURRENT_CONTROL_MODE = (uint8_t) msg.data[3];
FLAG_VALVE_DEADZONE = (uint8_t) msg.data[4];
- ROM_RESET_DATA();
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
+ spi_eeprom_write(RID_SENSING_MODE, (int16_t) SENSING_MODE);
+ spi_eeprom_write(RID_CURRENT_CONTROL_MODE, (int16_t) CURRENT_CONTROL_MODE);
+ spi_eeprom_write(RID_FLAG_VALVE_DEADZONE, (int16_t) FLAG_VALVE_DEADZONE);
break;
}
case CRX_SET_ENC_ZERO: {
@@ -110,6 +114,7 @@
case CRX_SET_CAN_FREQ: {
CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
ROM_RESET_DATA();
+ spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
break;
}
@@ -153,10 +158,10 @@
DIR_JOINT_ENC = 1;
else
DIR_JOINT_ENC = -1;
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
break;
}
@@ -173,10 +178,10 @@
DIR_VALVE = 1;
else
DIR_VALVE = -1;
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
break;
}
@@ -193,10 +198,10 @@
DIR_VALVE_ENC = 1;
else
DIR_VALVE_ENC = -1;
-
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
break;
}
@@ -209,10 +214,10 @@
case CRX_SET_VOLTAGE_SUPPLY: {
SUPPLY_VOLTAGE = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.));
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.0f));
break;
}
@@ -225,10 +230,10 @@
case CRX_SET_VOLTAGE_VALVE: {
VALVE_VOLTAGE_LIMIT = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.));
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f));
break;
@@ -251,34 +256,46 @@
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);
-
- ROM_RESET_DATA();
+
+// ROM_RESET_DATA();
+ 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);
-
- ROM_RESET_DATA();
-
+
+// ROM_RESET_DATA();
+ 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);
-
- ROM_RESET_DATA();
-
+
+// ROM_RESET_DATA();
+ 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);
+
} else if (msg.data[1] == 3) {
K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f);
D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f);
-
+
// ROM_RESET_DATA(); //For Real-time changing
+// spi_eeprom_write(RID_K_SPRING, (int16_t) K_SPRING);
+// spi_eeprom_write(RID_D_DAMPER, (int16_t) D_DAMPER);
+
} else if (msg.data[1] == 4) {
P_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[2] | msg.data[3] << 8);
I_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[4] | msg.data[5] << 8);
D_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[6] | msg.data[7] << 8);
-
- ROM_RESET_DATA();
+
+// ROM_RESET_DATA();
}
break;
@@ -294,8 +311,11 @@
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);
-
- ROM_RESET_DATA();
+
+// ROM_RESET_DATA();
+ 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));
break;
}
@@ -308,10 +328,10 @@
case CRX_SET_VELOCITY_COMP_GAIN: {
VELOCITY_COMP_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
break;
}
@@ -325,9 +345,9 @@
case CRX_SET_COMPLIANCE_GAIN: {
COMPLIANCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
break;
}
@@ -340,10 +360,10 @@
case CRX_SET_VALVE_FF: {
VALVE_FF = (int16_t) (msg.data[1] | msg.data[2] << 8);
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
break;
}
@@ -356,10 +376,10 @@
case CRX_SET_BULK_MODULUS: {
BULK_MODULUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
break;
}
@@ -374,10 +394,10 @@
CHAMBER_VOLUME_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
CHAMBER_VOLUME_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
- //spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
+ spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
break;
}
@@ -393,11 +413,11 @@
PISTON_AREA_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
PISTON_AREA_alpha = (double)PISTON_AREA_A/(double)PISTON_AREA_B;
alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
-
- ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
- //spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
+
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
+ spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
break;
}
@@ -406,16 +426,16 @@
// SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
//dac_1 = PRES_A_VREF;
//dac_2 = PRES_B_VREF;
-
+
break;
}
case CRX_SET_PRES: {
PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8);
PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
- //spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
+ spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
break;
@@ -430,9 +450,9 @@
case CRX_SET_ENC_LIMIT: {
ENC_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
ENC_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
- //spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
+ spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
break;
}
@@ -444,8 +464,8 @@
case CRX_SET_STROKE: {
STROKE = (int16_t) (msg.data[1] | msg.data[2] << 8);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
break;
}
@@ -459,7 +479,10 @@
case CRX_SET_VALVE_LIMIT: {
VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
- ROM_RESET_DATA();
+// ROM_RESET_DATA();
+
+ spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
+ spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
break;
}
@@ -472,8 +495,8 @@
case CRX_SET_ENC_PULSE_PER_POSITION: {
ENC_PULSE_PER_POSITION = (int16_t) (msg.data[1] | msg.data[2] << 8);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) ENC_PULSE_PER_POSITION);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) (ENC_PULSE_PER_POSITION*10.0f));
break;
}
@@ -486,8 +509,8 @@
case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.0001f);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) TORQUE_SENSOR_PULSE_PER_TORQUE);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
break;
}
@@ -501,9 +524,9 @@
case CRX_SET_PRES_SENSOR_PULSE_PER_PRES: {
PRES_SENSOR_A_PULSE_PER_BAR = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) * 0.01f;
PRES_SENSOR_B_PULSE_PER_BAR = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) * 0.01f;
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.));
- //(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.));
+// ROM_RESET_DATA();
+ 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));
break;
}
@@ -516,8 +539,8 @@
case CRX_SET_FRICTION: {
FRICTION = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.));
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.0f));
break;
}
@@ -533,12 +556,12 @@
VALVE_GAIN_LPM_PER_V[4] = (double) msg.data[3] / 50.0f;
VALVE_GAIN_LPM_PER_V[6] = (double) msg.data[4] / 50.0f;
VALVE_GAIN_LPM_PER_V[8] = (double) msg.data[5] / 50.0f;
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
break;
}
@@ -554,12 +577,12 @@
VALVE_GAIN_LPM_PER_V[5] = (double) msg.data[3] / 50.0f;
VALVE_GAIN_LPM_PER_V[7] = (double) msg.data[4] / 50.0f;
VALVE_GAIN_LPM_PER_V[9] = (double) msg.data[5] / 50.0f;
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
- //spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
- //(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
+ spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
break;
}
@@ -605,8 +628,8 @@
}
case CRX_SET_HOMEPOS_OFFSET: {
HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
break;
}
@@ -616,8 +639,8 @@
}
case CRX_SET_HOMEPOS_VALVE_OPENING: {
HOMEPOS_VALVE_OPENING = (int16_t) (msg.data[1] | msg.data[2] << 8);
- ROM_RESET_DATA();
- //spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
break;
}
@@ -635,7 +658,7 @@
CAN_TX_VALVE_POS_NUM();
break;
}
-
+
case CRX_SET_ROM: {
ROM_RESET_DATA();
break;
@@ -643,19 +666,24 @@
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;
- ROM_RESET_DATA();
+// ROM_RESET_DATA();
+ 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;
- ROM_RESET_DATA();
+// ROM_RESET_DATA();
+ 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;
- ROM_RESET_DATA();
+// ROM_RESET_DATA();
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
break;
}
case CRX_SET_PID_GAIN_OPP: {
@@ -663,9 +691,9 @@
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);
-
- ROM_RESET_DATA();
-
+
+// ROM_RESET_DATA();
+
//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);
@@ -675,8 +703,8 @@
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);
-
- ROM_RESET_DATA();
+
+// ROM_RESET_DATA();
//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);
@@ -685,9 +713,9 @@
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);
-
- ROM_RESET_DATA();
-
+
+// ROM_RESET_DATA();
+
//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);
@@ -699,7 +727,7 @@
CAN_TX_DDV_VALVE_MAX_MIN_POS();
break;
}
- case CRX_DELAY_TEST:{
+ case CRX_DELAY_TEST: {
flag_delay_test = 1;
break;
}
@@ -713,75 +741,74 @@
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;
}
- default:
- break;
+ default:
+ break;
}
}
void CAN_RX_HANDLER()
{
-
+
can.read(msg);
unsigned int address = msg.id;
- if(address==CID_RX_CMD){
+ if(address==CID_RX_CMD) {
unsigned int CMD = msg.data[0];
ReadCMD(CMD);
-
+
} else if(address==CID_RX_REF_POSITION) {
-
+
int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 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;
- }
- else { //Linear Actuator
+ } else { //Linear Actuator
pos.ref = (double)temp_pos * 10.0f;
vel.ref = (double)temp_vel * 256.0f;
}
-
+
torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N
torq.ref_diff = torq.ref - torq.ref_old;
torq.ref_old = torq.ref;
-
+
///////////////Make Data///////////////////
- for(int i=0; i<num_array_u_past-1;i++){
+ for(int i=0; i<num_array_u_past-1; i++) {
u_past[i] = u_past[i+1];
}
u_past[num_array_u_past-1] = output_normalized;
-
- for(int i=0; i<num_array_x_past-1;i++){
+
+ for(int i=0; i<num_array_x_past-1; i++) {
x_past[i] = x_past[i+1];
}
x_past[num_array_x_past-1] = pos.sen / ENC_PULSE_PER_POSITION; //mm
-
+
// for(int i=0;i<num_array_x_future-1;i++){
// x_future[i] = x_future[i+1];
// }
// x_future[num_array_x_future-1] = pos.sen / ENC_PULSE_PER_POSITION; //mm
-
- for(int i=0; i<num_array_f_past-1;i++){
+
+ for(int i=0; i<num_array_f_past-1; i++) {
f_past[i] = f_past[i+1];
}
f_past[num_array_f_past-1] = torq.sen; //N
-
+
f_future[0] = torq.sen; //N
- for(int i=1;i<num_array_f_future-1;i++){
+ for(int i=1; i<num_array_f_future-1; i++) {
f_future[i] = f_future[i+1];
}
f_future[num_array_f_future-1] = torq.ref; //N
@@ -792,7 +819,7 @@
// input_NN[ind] = u_past[1*i];
// ind = ind + 1;
// }
-//
+//
// for(int i=0;i<numpast_x;i++){
// input_NN[ind] = x_past[1*i] / 60.0f;
// ind = ind + 1;
@@ -803,7 +830,7 @@
//// input_NN[ind] = x_future[2*i+2] / 60.0f;
//// ind = ind + 1;
//// }
-//
+//
// for(int i=0;i<numpast_f;i++){
// input_NN[ind] = f_past[1*i] / 10000.0f + 0.5f;
// ind = ind + 1;
@@ -815,10 +842,10 @@
//// input_NN[ind] = (f_future[2*i+2])/10000.0f+0.5f;
// ind = ind + 1;
// }
-
+
} else if(address==CID_RX_REF_VALVE_POS) {
int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
-
+
if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve
valve_pos.ref = (double) temp_ref_valve_pos;
} else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
@@ -830,19 +857,20 @@
valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
}
}
- } else if(address==CID_RX_REF_PWM){
+ } else if(address==CID_RX_REF_PWM) {
int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
Vout.ref = (double) temp_ref_pwm;
- }
+ }
}
/******************************************************************************
Information Transmission Functions
*******************************************************************************/
-void CAN_TX_INFO(void) {
+void CAN_TX_INFO(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
@@ -856,9 +884,10 @@
can.write(temp_msg);
}
-void CAN_TX_BNO(void) {
+void CAN_TX_BNO(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
@@ -867,9 +896,10 @@
can.write(temp_msg);
}
-void CAN_TX_OPERATING_MODE(void) {
+void CAN_TX_OPERATING_MODE(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE;
@@ -881,7 +911,8 @@
can.write(temp_msg);
}
-void CAN_TX_CAN_FREQ(void) {
+void CAN_TX_CAN_FREQ(void)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -893,7 +924,8 @@
can.write(temp_msg);
}
-void CAN_TX_CONTROL_MODE(void) {
+void CAN_TX_CONTROL_MODE(void)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -904,7 +936,8 @@
can.write(temp_msg);
}
-void CAN_TX_JOINT_ENC_DIR(void) {
+void CAN_TX_JOINT_ENC_DIR(void)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -916,7 +949,8 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_DIR(void) {
+void CAN_TX_VALVE_DIR(void)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -928,7 +962,8 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_ENC_DIR(void) {
+void CAN_TX_VALVE_ENC_DIR(void)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -940,7 +975,8 @@
can.write(temp_msg);
}
-void CAN_TX_VOLTAGE_SUPPLY(void) {
+void CAN_TX_VOLTAGE_SUPPLY(void)
+{
int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10.0f);
CANMessage temp_msg;
@@ -954,7 +990,8 @@
can.write(temp_msg);
}
-void CAN_TX_VOLTAGE_VALVE(void) {
+void CAN_TX_VOLTAGE_VALVE(void)
+{
int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f);
CANMessage temp_msg;
@@ -969,7 +1006,8 @@
}
-void CAN_TX_PID_GAIN(int t_type) {
+void CAN_TX_PID_GAIN(int t_type)
+{
// t_type = 0 : valve position control gain
// t_type = 1 : joint position control gain
// t_type = 2 : joint torque control gain
@@ -993,7 +1031,7 @@
}
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 8;
temp_msg.data[0] = (uint8_t) CTX_SEND_PID_GAIN;
@@ -1009,7 +1047,8 @@
}
-void CAN_TX_VALVE_DEADZONE(void) {
+void CAN_TX_VALVE_DEADZONE(void)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -1025,9 +1064,10 @@
can.write(temp_msg);
}
-void CAN_TX_VELOCITY_COMP_GAIN(void) {
+void CAN_TX_VELOCITY_COMP_GAIN(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VELOCITY_COMP_GAIN;
@@ -1037,9 +1077,10 @@
can.write(temp_msg);
}
-void CAN_TX_COMPLIANCE_GAIN(void) {
+void CAN_TX_COMPLIANCE_GAIN(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_COMPLIANCE_GAIN;
@@ -1049,9 +1090,10 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_FF(void) {
+void CAN_TX_VALVE_FF(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_FF;
@@ -1061,9 +1103,10 @@
can.write(temp_msg);
}
-void CAN_TX_BULK_MODULUS(void) {
+void CAN_TX_BULK_MODULUS(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_BULK_MODULUS;
@@ -1073,9 +1116,10 @@
can.write(temp_msg);
}
-void CAN_TX_CHAMBER_VOLUME(void) {
+void CAN_TX_CHAMBER_VOLUME(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_CHAMBER_VOLUME;
@@ -1087,9 +1131,10 @@
can.write(temp_msg);
}
-void CAN_TX_PISTON_AREA(void) {
+void CAN_TX_PISTON_AREA(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_PISTON_AREA;
@@ -1101,9 +1146,10 @@
can.write(temp_msg);
}
-void CAN_TX_PRES_A_AND_B(void) {
+void CAN_TX_PRES_A_AND_B(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_PRES;
@@ -1115,9 +1161,10 @@
can.write(temp_msg);
}
-void CAN_TX_ENC_LIMIT(void) {
+void CAN_TX_ENC_LIMIT(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_LIMIT;
@@ -1129,9 +1176,10 @@
can.write(temp_msg);
}
-void CAN_TX_STROKE(void) {
+void CAN_TX_STROKE(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
@@ -1141,9 +1189,10 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_LIMIT(void) {
+void CAN_TX_VALVE_LIMIT(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
@@ -1155,9 +1204,10 @@
can.write(temp_msg);
}
-void CAN_TX_ENC_PULSE_PER_POSITION(void) {
+void CAN_TX_ENC_PULSE_PER_POSITION(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
@@ -1168,9 +1218,10 @@
can.write(temp_msg);
}
-void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void) {
+void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
@@ -1181,9 +1232,10 @@
can.write(temp_msg);
}
-void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void) {
+void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
@@ -1195,11 +1247,12 @@
can.write(temp_msg);
}
-void CAN_TX_FRICTION(void) {
+void CAN_TX_FRICTION(void)
+{
CANMessage temp_msg;
int16_t send_friction;
send_friction = (int16_t) (FRICTION * 10.0f);
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_SEND_FRICTION;
@@ -1209,9 +1262,10 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_GAIN_PLUS(void) {
+void CAN_TX_VALVE_GAIN_PLUS(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS;
@@ -1224,18 +1278,19 @@
can.write(temp_msg);
}
-void CAN_TX_DDV_VALVE_DEADZONE(void) {
+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 = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
//temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
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 = 7;
temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE;
@@ -1249,9 +1304,10 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_GAIN_MINUS(void) {
+void CAN_TX_VALVE_GAIN_MINUS(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS;
@@ -1264,9 +1320,10 @@
can.write(temp_msg);
}
-void CAN_TX_REFENCE_MODE(void) {
+void CAN_TX_REFENCE_MODE(void)
+{
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 6;
temp_msg.data[0] = (uint8_t) CTX_SEND_REFENCE_MODE;
@@ -1279,11 +1336,12 @@
can.write(temp_msg);
}
-void CAN_TX_HOMEPOS_OFFSET(void) {
+void CAN_TX_HOMEPOS_OFFSET(void)
+{
CANMessage temp_msg;
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] = (uint8_t) CTX_SEND_HOMEPOS_OFFSET;
@@ -1293,11 +1351,12 @@
can.write(temp_msg);
}
-void CAN_TX_HOMPOS_VALVE_OPENING(void) {
+void CAN_TX_HOMPOS_VALVE_OPENING(void)
+{
CANMessage temp_msg;
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] = (uint8_t) CTX_SEND_HOMEPOS_VALVE_OPENING;
@@ -1307,20 +1366,21 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t 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]);
-
+
if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
} else {
valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
}
-
+
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] = (uint8_t) CTX_VALVE_PWM_VS_VALVE_POS;
@@ -1332,7 +1392,8 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t 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]);
@@ -1344,7 +1405,7 @@
} else {
temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER));
}
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 8;
temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
@@ -1358,11 +1419,12 @@
can.write(temp_msg);
}
-void CAN_TX_VALVE_POS_NUM(void) {
+void CAN_TX_VALVE_POS_NUM(void)
+{
CANMessage temp_msg;
int32_t valve_pos_num;
valve_pos_num = (int16_t) VALVE_POS_NUM;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_NUM;
@@ -1372,17 +1434,18 @@
can.write(temp_msg);
}
-void CAN_TX_DDV_VALVE_MAX_MIN_POS(void) {
+void CAN_TX_DDV_VALVE_MAX_MIN_POS(void)
+{
CANMessage temp_msg;
-
+
float temp_valve_max_pos = 0.0f;
float temp_valve_min_pos = 0.0f;
float temp_ddv_center = 0.0f;
-
+
temp_valve_max_pos = VALVE_MAX_POS;
temp_valve_min_pos = VALVE_MIN_POS;
temp_ddv_center = VALVE_CENTER;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
@@ -1397,12 +1460,13 @@
}
-void CAN_TX_BUFFER(int16_t t_cnt_buffer) {
+void CAN_TX_BUFFER(int16_t t_cnt_buffer)
+{
CANMessage temp_msg;
int16_t send_pos_array, send_ref_array;
send_pos_array = (int16_t) (pos_array[t_cnt_buffer]);
send_ref_array = (int16_t) (ref_array[t_cnt_buffer]);
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (uint8_t) CTX_SEND_BUFFER;
@@ -1419,7 +1483,8 @@
Sensor & State Transmission Functions
*******************************************************************************/
-void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq) {
+void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_POSITION;
@@ -1434,8 +1499,9 @@
can.write(temp_msg);
}
-void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb) {
-
+void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb)
+{
+
CANMessage temp_msg;
temp_msg.id = CID_TX_POSITION;
@@ -1465,7 +1531,8 @@
// can.write(temp_msg);
//}
-void CAN_TX_TORQUE(int16_t t_valve_pos) {
+void CAN_TX_TORQUE(int16_t t_valve_pos)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_TORQUE;
@@ -1476,7 +1543,8 @@
can.write(temp_msg);
}
-void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) {
+void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_PRES;
@@ -1489,7 +1557,8 @@
can.write(temp_msg);
}
-void CAN_TX_PWM(int16_t t_pwm) {
+void CAN_TX_PWM(int16_t t_pwm)
+{
CANMessage temp_msg;
temp_msg.id = CID_TX_VOUT;
@@ -1514,7 +1583,7 @@
temp_msg.data[5] = (uint8_t) (t_ref_valve_pos >> 8);
temp_msg.data[6] = (uint8_t) t_pwm;
temp_msg.data[7] = (uint8_t) (t_pwm >> 8);
-
+
can.write(temp_msg);
}
\ No newline at end of file