Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 232:2976cf1b4252, committed 2021-05-31
- Comitter:
- Lightvalve
- Date:
- Mon May 31 07:19:49 2021 +0000
- Parent:
- 231:e00e71ca3e80
- Child:
- 233:2ec0de8590d3
- Commit message:
- 210531
Changed in this revision
--- a/CAN/function_CAN.cpp Tue Apr 27 11:31:49 2021 +0000 +++ b/CAN/function_CAN.cpp Mon May 31 07:19:49 2021 +0000 @@ -506,8 +506,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); - spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f)); + TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f); + spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*100.0f)); break; } @@ -671,12 +671,12 @@ int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8); if((OPERATING_MODE&0b001)==0) { // Rotary Actuator - 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 + REF_POSITION = (float)temp_pos / 200.0f; + REF_VELOCITY = (float)temp_vel / 20.0f; + REF_TORQUE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> Nm } else { //Linear Actuator - 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_POSITION = (float)temp_pos / 200.0f; + REF_VELOCITY = (float)temp_vel / 20.0f; REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> N } @@ -1107,7 +1107,7 @@ temp_msg.id = CID_TX_INFO; 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); + int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f); temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque; temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
--- a/function_utilities/function_utilities.cpp Tue Apr 27 11:31:49 2021 +0000 +++ b/function_utilities/function_utilities.cpp Mon May 31 07:19:49 2021 +0000 @@ -437,7 +437,7 @@ ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS); STROKE = spi_eeprom_read(RID_STROKE); ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION)); - TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f; + TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.01f; PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f; // PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f; PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
--- a/main.cpp Tue Apr 27 11:31:49 2021 +0000 +++ b/main.cpp Mon May 31 07:19:49 2021 +0000 @@ -238,7 +238,7 @@ make_delay(); ////// bno rom -// spi_eeprom_write(RID_BNO, (int16_t) 4); +// spi_eeprom_write(RID_BNO, (int16_t) 2); // make_delay(); //////// @@ -457,20 +457,20 @@ float pres_A_new, pres_B_new; if (DIR_VALVE_ENC > 0) { pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar - pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; + pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; } else { pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar - pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; + pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; } pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f); pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f); if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm - torq.UpdateSen(torq_new,FREQ_TMR4,1000.0); // unit : Nm + torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f); // unit : Nm } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N - force.UpdateSen(force_new,FREQ_TMR4,1000.0); // unit : N + force.UpdateSen(force_new,FREQ_TMR4,1000.0f); // unit : N } } @@ -576,7 +576,7 @@ static float PresB_pulse_sum = 0.0; // DAC Voltage reference set - float VREF_TuningGain = 0.000003f; + float VREF_TuningGain = -0.000003f; if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) { LED = 1; if(SENSING_MODE == 0) { // Force Sensor (Loadcell) @@ -684,10 +684,16 @@ } } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) { - int T_move = 2*TMR_FREQ_5k; - REF_POSITION_FINDHOME = (0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT; - +// int T_move = 2*TMR_FREQ_5k; + int T_move = 10000; + REF_POSITION_FINDHOME = ((0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT)/ENC_PULSE_PER_POSITION; + cnt_findhome++; + + REFERENCE_MODE = MODE_REF_FINDHOME; + CONTROL_MODE = MODE_JOINT_CONTROL; + alpha_trans = 0.0f; + if (cnt_findhome >= T_move) { cnt_findhome = 0; pos.ref = 0.0f; @@ -1004,9 +1010,11 @@ // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*8.0f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); +// CAN_TX_POSITION_FT((int16_t) (PRES_A_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_A.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*8.0f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); +// CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } }