Sungwoo Kim
/
HydraulicControlBoard_PostLIGHT_Original
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 230:2c3e5ecbe7e1
- Parent:
- 227:699c3e572283
- Child:
- 231:e00e71ca3e80
diff -r 7a1c46b9b471 -r 2c3e5ecbe7e1 CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Tue Apr 20 00:42:45 2021 +0000 +++ b/CAN/function_CAN.cpp Wed Apr 21 04:20:39 2021 +0000 @@ -14,10 +14,11 @@ int CID_TX_INFO = 1100; int CID_TX_POS_VEL_TORQ = 1200; int CID_TX_PWM = 1300; +int CID_TX_SOMETHING = 1400; + int CID_TX_CURRENT = 1400; int CID_TX_VOUT = 1500; int CID_TX_VALVE_POSITION = 1600; -int CID_TX_SOMETHING = 1700; // variables uint8_t can_index = 0; @@ -32,6 +33,31 @@ extern float input_NN[]; /******************************************************************************* + * State Class functions + ******************************************************************************/ +void State::UpdateSen(float sen_new, float Freq_update, float f_cut) { + if(f_cut<=0.0f) f_cut=0.001f; + + this->sen_diff = (sen_new-this->sen)*Freq_update; + float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz + this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new; +} + +void State::UpdateRef(float ref_new) { + this->ref = ref_new; +} + +void State::Reset() { + this->sen = 0.0f; + this->sen_diff = 0.0f; + this->ref = 0.0f; + this->err = 0.0f; + this->err_int = 0.0f; + this->err_old = 0.0f; + this->err_diff = 0.0f; +} + +/******************************************************************************* * CAN functions ******************************************************************************/ void CAN_ID_INIT(void) @@ -232,6 +258,18 @@ CONTROL_UTILITY_MODE = 22; break; } + + case CRX_ASK_VARIABLE_SUPPLY: + { + CAN_TX_VARIABLE_SUPPLY_ONOFF(); + break; + } + + case CRX_SET_VARIABLE_SUPPLY: + { + SUPPLY_PRESSURE_UPDATE = msg.data[1]; + break; + } case CRX_ASK_PID_GAIN: { CAN_TX_PID_GAIN(msg.data[1]); @@ -392,22 +430,16 @@ break; } - case CRX_ASK_PRES: { - CAN_TX_PRES_A_AND_B(); -// SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); - //dac_1 = PRES_A_VREF; - //dac_2 = PRES_B_VREF; - + case CRX_ASK_SUP_PRES: { + CAN_TX_SUP_PRES(); 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); - spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY); - spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN); - - + case CRX_SET_SUP_PRES: { + int16_t temp = (int16_t) (msg.data[1] | msg.data[2] << 8); + spi_eeprom_write(RID_PRES_SUPPLY, temp); + PRES_SUPPLY_NOM = (float)temp; + PRES_SUPPLY = PRES_SUPPLY_NOM; break; } @@ -634,65 +666,58 @@ } else if(address==CID_RX_REF_POSITION) { - int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); + 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 - REF_POSITION = (double)temp_pos * 10.0f; - REF_VELOCITY = (double)temp_vel * 256.0f; + REF_POSITION = (float)temp_pos * 1.0f / ENC_PULSE_PER_POSITION; // pulse >> deg + REF_VELOCITY = (float)temp_vel * 10.0f / ENC_PULSE_PER_POSITION; // pulse/s >> deg/s + REF_TORQUE = (float)temp_torq * 0.01f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> Nm } else { //Linear Actuator - REF_POSITION = (double)temp_pos * 10.0f; - REF_VELOCITY = (double)temp_vel * 256.0f; + REF_POSITION = (float)temp_pos * 4.0f / ENC_PULSE_PER_POSITION; // pulse >> mm + REF_VELOCITY = (float)temp_vel * 100.0f / ENC_PULSE_PER_POSITION; // pulse/s >> mm/s + REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> N } - 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(SUPPLY_PRESSURE_UPDATE == 1) { + int16_t temp_REF_Ps = (int16_t) (msg.data[6] | msg.data[7] << 8); + PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0; + if(PRES_SUPPLY<35.0f) PRES_SUPPLY = 35.0f; + else if(PRES_SUPPLY>210.0f) PRES_SUPPLY = 210.0f; + } else { + PRES_SUPPLY = PRES_SUPPLY_NOM; + } 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)); - } + CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*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)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } + CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } } - // PWM, ID:1300 + // Valve Position (ID:1300) if (flag_data_request[1] == HIGH) { - CAN_TX_PWM((int16_t) (V_out)); //1300 + CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); } - // Reference Current, Current Current (ID:1400) + // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) if (flag_data_request[2] == HIGH) { - CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400 + CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (0), (int16_t) (0)); } - - // Reference Current, Current Current (ID:1700) - if (flag_data_request[3] == HIGH) { - CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700 - } - } - } 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 - valve_pos.ref = (double) temp_ref_valve_pos; + valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~10000) } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve - valve_pos.ref = (double) temp_ref_valve_pos; + valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000) } else { //SW Valve if(temp_ref_valve_pos >= 0) { valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f; @@ -851,6 +876,16 @@ can.write(temp_msg); } +void CAN_TX_VARIABLE_SUPPLY_ONOFF(void) +{ + CANMessage temp_msg; + temp_msg.id = CID_TX_INFO; + temp_msg.len = 2; + temp_msg.data[0] = (uint8_t) CTX_SEND_VARIABLE_SUPPLY; + temp_msg.data[1] = (uint8_t) SUPPLY_PRESSURE_UPDATE; + + can.write(temp_msg); +} void CAN_TX_PID_GAIN(int t_type) { @@ -992,17 +1027,18 @@ can.write(temp_msg); } -void CAN_TX_PRES_A_AND_B(void) +void CAN_TX_SUP_PRES(void) { CANMessage temp_msg; + int16_t temp_PRES_SUPPLY = (int16_t) (PRES_SUPPLY); temp_msg.id = CID_TX_INFO; 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); - temp_msg.data[3] = (uint8_t) PRES_RETURN; - temp_msg.data[4] = (uint8_t) (PRES_RETURN >> 8); + temp_msg.data[0] = (uint8_t) CTX_SEND_SUP_PRES; + temp_msg.data[1] = (uint8_t) temp_PRES_SUPPLY; + temp_msg.data[2] = (uint8_t) (temp_PRES_SUPPLY >> 8); + temp_msg.data[3] = 0; + temp_msg.data[4] = 0; can.write(temp_msg); }