![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 225:9c0becc196ba
- Parent:
- 224:985dba42f261
- Child:
- 226:82a3ca333004
diff -r 985dba42f261 -r 9c0becc196ba CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Mon Dec 28 14:27:11 2020 +0000 +++ b/CAN/function_CAN.cpp Thu Feb 04 05:43:47 2021 +0000 @@ -17,6 +17,7 @@ int CID_TX_PRES = 1400; int CID_TX_VOUT = 1500; int CID_TX_VALVE_POSITION = 1600; +int CID_TX_SOMETHING = 1700; // variables uint8_t can_index = 0; @@ -47,6 +48,7 @@ CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES); CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT); CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION); + CID_TX_SOMETHING = (int) (BNO + INIT_CID_TX_SOMETHING); } void ReadCMD(int16_t CMD) @@ -731,6 +733,69 @@ 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)); + } + } + } + + // 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; +// if(value>=(float) VALVE_CENTER) { +// t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); +// } else { +// t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); +// } +// double t_value_ref = 0.0f; +// if(valve_pos.ref>=(float) VALVE_CENTER) { +// t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); +// } 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++) { + ; + } + + // 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) { @@ -751,6 +816,8 @@ int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8); Vout.ref = (double) temp_ref_pwm; } + + } /****************************************************************************** @@ -762,7 +829,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 7; + temp_msg.len = 8; temp_msg.data[0] = (uint8_t) CTX_SEND_INFO; temp_msg.data[1] = (uint8_t) BNO; temp_msg.data[2] = (uint8_t) CAN_FREQ; @@ -779,7 +846,8 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 2; + //temp_msg.len = 2; + temp_msg.len = 8; temp_msg.data[0] = (uint8_t) CTX_SEND_BNO; temp_msg.data[1] = (uint8_t) BNO; @@ -791,7 +859,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 8; 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; @@ -806,7 +874,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 8; 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); @@ -819,7 +887,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 2; + temp_msg.len = 8; temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE; temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE; @@ -831,7 +899,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -844,7 +912,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -857,7 +925,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -872,7 +940,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -887,7 +955,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -942,7 +1010,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 7; + temp_msg.len = 8; 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); @@ -959,7 +1027,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -972,7 +1040,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -985,7 +1053,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -998,7 +1066,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -1011,7 +1079,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 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); @@ -1026,7 +1094,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 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); @@ -1041,7 +1109,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 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); @@ -1056,7 +1124,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 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); @@ -1071,7 +1139,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -1084,7 +1152,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 8; 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); @@ -1099,7 +1167,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 8; 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; @@ -1113,7 +1181,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 8; 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; @@ -1127,7 +1195,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 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.0f); temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f) >> 8); @@ -1144,7 +1212,7 @@ send_friction = (int16_t) (FRICTION * 10.0f); temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -1157,7 +1225,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 6; + temp_msg.len = 8; 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); @@ -1180,7 +1248,7 @@ temp_ddv_center = (double)VALVE_CENTER; temp_msg.id = CID_TX_INFO; - temp_msg.len = 7; + 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); @@ -1197,7 +1265,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 6; + temp_msg.len = 8; 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); @@ -1213,7 +1281,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 6; + temp_msg.len = 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.0f); @@ -1231,7 +1299,7 @@ send_homepos_offset = (int16_t) (HOMEPOS_OFFSET); temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -1246,7 +1314,7 @@ send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING); temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -1269,7 +1337,7 @@ PWM_VALVE_ID = ID_index_array[canindex] * 1000; temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 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); @@ -1298,10 +1366,10 @@ 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); - temp_msg.data[5] = (uint8_t) valve_pos_vs_flowrate; - temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 8); - temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 16); - temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 24); + 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); } @@ -1313,7 +1381,7 @@ valve_pos_num = (int16_t) VALVE_POS_NUM; temp_msg.id = CID_TX_INFO; - temp_msg.len = 3; + temp_msg.len = 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); @@ -1334,7 +1402,7 @@ temp_ddv_center = VALVE_CENTER; temp_msg.id = CID_TX_INFO; - temp_msg.len = 7; + temp_msg.len = 8; 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); @@ -1355,7 +1423,7 @@ send_ref_array = (int16_t) (ref_array[t_cnt_buffer]); temp_msg.id = CID_TX_INFO; - temp_msg.len = 5; + temp_msg.len = 8; 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); @@ -1375,7 +1443,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_POSITION; - temp_msg.len = 6; + temp_msg.len = 8; 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; @@ -1411,7 +1479,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_TORQUE; - temp_msg.len = 2; + temp_msg.len = 8; temp_msg.data[0] = (uint8_t) t_valve_pos; temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8); @@ -1423,7 +1491,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_PRES; - temp_msg.len = 4; + temp_msg.len = 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; @@ -1437,7 +1505,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_VOUT; - temp_msg.len = 2; + temp_msg.len = 8; temp_msg.data[0] = (uint8_t) t_pwm; temp_msg.data[1] = (uint8_t) (t_pwm >> 8); @@ -1461,4 +1529,23 @@ can.write(temp_msg); +} + +void CAN_TX_SOMETHING(int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d) +{ + CANMessage temp_msg; + + temp_msg.id = CID_TX_SOMETHING; + temp_msg.len = 8; + temp_msg.data[0] = (uint8_t) t_a; + temp_msg.data[1] = (uint8_t) (t_a >> 8); + temp_msg.data[2] = (uint8_t) t_b; + temp_msg.data[3] = (uint8_t) (t_b >> 8); + temp_msg.data[4] = (uint8_t) t_c; + temp_msg.data[5] = (uint8_t) (t_c >> 8); + temp_msg.data[6] = (uint8_t) t_d; + temp_msg.data[7] = (uint8_t) (t_d >> 8); + + + can.write(temp_msg); } \ No newline at end of file