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