Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy1
2011
Diff: main.cpp
- Revision:
- 43:b084e5f5d0d5
- Parent:
- 42:1cf66990ccab
- Child:
- 44:fe7d5cfd2eea
--- a/main.cpp Thu Dec 26 00:19:14 2019 +0000 +++ b/main.cpp Tue Jan 07 08:39:22 2020 +0000 @@ -206,8 +206,10 @@ make_delay(); //DAC init - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; +// dac_1 = PRES_A_VREF / 3.3f; +// dac_2 = PRES_B_VREF / 3.3f; + dac_1 = TORQUE_VREF / 3.3f; + dac_2 = 0.0f; make_delay(); for (int i=0; i<50; i++) { @@ -412,7 +414,7 @@ // float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR; float pres_A_new = ((float)ADC1->DR); pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); - torq.sen = - (pres_A.sen-2048.0f)*10000.0f / 2048.0f; //N + torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047 //Pressure sensor 1B @@ -833,13 +835,11 @@ CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f; CUR_TORQUE_sum = 0; - TORQUE_VREF += 0.0001f * (TORQUE_NULL - CUR_TORQUE_mean); + TORQUE_VREF += 0.0003 * (0.0f - CUR_TORQUE_mean); if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f; if (TORQUE_VREF < 0) TORQUE_VREF = 0; - - ROM_RESET_DATA(); - + //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); dac_1 = TORQUE_VREF / 3.3f; } @@ -859,6 +859,53 @@ } TMR3_COUNT_TORQUE_NULL++; break; + + + + // // DAC Voltage reference set +// if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) { +// CUR_PRES_A_sum += pres_A.sen; +// CUR_PRES_B_sum += pres_B.sen; +// +// if (TMR3_COUNT_PRES_NULL % 10 == 0) { +// CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f; +// CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f; +// CUR_PRES_A_sum = 0; +// CUR_PRES_B_sum = 0; +// +// float VREF_NullingGain = 0.0003f; +// PRES_A_VREF = PRES_A_VREF - VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean); +// PRES_B_VREF = PRES_B_VREF - VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean); +// +// if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; +// if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; +// if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; +// if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; +// +// dac_1 = PRES_A_VREF / 3.3f; +// dac_2 = PRES_B_VREF / 3.3f; +// } +// } else { +// CONTROL_MODE = MODE_NO_ACT; +// TMR3_COUNT_PRES_NULL = 0; +// CUR_PRES_A_sum = 0; +// CUR_PRES_B_sum = 0; +// CUR_PRES_A_mean = 0; +// CUR_PRES_B_mean = 0; +// +// ROM_RESET_DATA(); +// +// dac_1 = PRES_A_VREF / 3.3f; +// dac_2 = PRES_B_VREF / 3.3f; +// //pc.printf("nulling end"); +// } +// TMR3_COUNT_PRES_NULL++; +// break; + + + + + } case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { @@ -1677,7 +1724,7 @@ if (flag_data_request[0] == HIGH) { //position+velocity //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); - CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) torq.sen); + CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f)); } if (flag_data_request[1] == HIGH) {