Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_
rainbow
Diff: main.cpp
- Revision:
- 232:2976cf1b4252
- Parent:
- 231:e00e71ca3e80
- Child:
- 233:2ec0de8590d3
diff -r e00e71ca3e80 -r 2976cf1b4252 main.cpp --- 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)); } }