Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy1
2011
Diff: CAN/function_CAN.cpp
- Revision:
- 170:42c938a40313
- Parent:
- 169:645207e160ca
- Child:
- 177:8e9cf31d63f4
diff -r 645207e160ca -r 42c938a40313 CAN/function_CAN.cpp --- 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