Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_
rainbow
Diff: CAN/function_CAN.cpp
- Revision:
- 227:699c3e572283
- Parent:
- 226:82a3ca333004
- Child:
- 230:2c3e5ecbe7e1
--- a/CAN/function_CAN.cpp Wed Mar 03 10:56:25 2021 +0000 +++ b/CAN/function_CAN.cpp Thu Mar 11 07:27:23 2021 +0000 @@ -8,13 +8,13 @@ // CAN ID Setting Variables int CID_RX_CMD = 100; int CID_RX_REF_POSITION = 200; -int CID_RX_REF_VALVE_POS = 300; +int CID_RX_REF_OPENLOOP = 300; int CID_RX_REF_PWM = 400; int CID_TX_INFO = 1100; -int CID_TX_POSITION = 1200; -int CID_TX_TORQUE = 1300; -int CID_TX_PRES = 1400; +int CID_TX_POS_VEL_TORQ = 1200; +int CID_TX_PWM = 1300; +int CID_TX_CURRENT = 1400; int CID_TX_VOUT = 1500; int CID_TX_VALVE_POSITION = 1600; int CID_TX_SOMETHING = 1700; @@ -39,13 +39,13 @@ CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD); CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION); - CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS); + CID_RX_REF_OPENLOOP = (int) (BNO + INIT_CID_RX_REF_OPENLOOP); CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM); CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO); - CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION); - CID_TX_TORQUE = (int) (BNO + INIT_CID_TX_TORQUE); - CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES); + CID_TX_POS_VEL_TORQ = (int) (BNO + INIT_CID_TX_POS_VEL_TORQ); + CID_TX_PWM = (int) (BNO + INIT_CID_TX_PWM); + CID_TX_CURRENT = (int) (BNO + INIT_CID_TX_CURRENT); 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); @@ -140,6 +140,7 @@ // if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3; + break; } @@ -638,84 +639,54 @@ 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 * 10.0f; - vel.ref = (double)temp_vel * 256.0f; + REF_POSITION = (double)temp_pos * 10.0f; + REF_VELOCITY = (double)temp_vel * 256.0f; } else { //Linear Actuator - pos.ref = (double)temp_pos * 10.0f; - vel.ref = (double)temp_vel * 256.0f; + REF_POSITION = (double)temp_pos * 10.0f; + REF_VELOCITY = (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_dot.sen = torq.sen-torq_sen_past; - torq.ref_old = torq.ref; - torq_sen_past = torq.sen; + REF_TORQUE = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N + torq.ref_diff = REF_TORQUE - REF_TORQUE_OLD; + REF_TORQUE_OLD = REF_TORQUE; 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)); + 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/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f)); + 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)*10.0f), (int16_t) ((pres_B.sen)*10.0f)); + 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 + // PWM, ID:1300 if (flag_data_request[1] == HIGH) { - CAN_TX_TORQUE((int16_t) (pos.ref/10.0f)); //1300 + CAN_TX_PWM((int16_t) (V_out)); //1300 } - // Reference Current, Current Current (ID:1700) + // Reference Current, Current Current (ID:1400) 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 { -// 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 + CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400 } - - //If it doesn't rest, below can can not work. - for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) { - ; - } - - // ID:1500 + + // Reference Current, Current Current (ID:1700) 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 + CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700 } } - } else if(address==CID_RX_REF_VALVE_POS) { + } else if(address==CID_RX_REF_OPENLOOP) { int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve @@ -1336,7 +1307,7 @@ { CANMessage temp_msg; - temp_msg.id = CID_TX_POSITION; + temp_msg.id = CID_TX_POS_VEL_TORQ; temp_msg.len = 6; temp_msg.data[0] = (uint8_t) t_pos; temp_msg.data[1] = (uint8_t) (t_pos >> 8); @@ -1353,7 +1324,7 @@ CANMessage temp_msg; - temp_msg.id = CID_TX_POSITION; + temp_msg.id = CID_TX_POS_VEL_TORQ; temp_msg.len = 8; temp_msg.data[0] = (uint8_t) t_pos; temp_msg.data[1] = (uint8_t) (t_pos >> 8); @@ -1367,24 +1338,11 @@ can.write(temp_msg); } - -void CAN_TX_TORQUE(int16_t t_torque) +void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b) { CANMessage temp_msg; - temp_msg.id = CID_TX_TORQUE; - 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); -} - -void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) -{ - CANMessage temp_msg; - - temp_msg.id = CID_TX_PRES; + temp_msg.id = CID_TX_CURRENT; temp_msg.len = 4; temp_msg.data[0] = (uint8_t) t_pres_a; temp_msg.data[1] = (uint8_t) (t_pres_a >> 8); @@ -1398,7 +1356,7 @@ { CANMessage temp_msg; - temp_msg.id = CID_TX_VOUT; + temp_msg.id = CID_TX_PWM; temp_msg.len = 2; temp_msg.data[0] = (uint8_t) t_pwm; temp_msg.data[1] = (uint8_t) (t_pwm >> 8);