Kim GiJeong
/
HydraulicControlBoard_LIGHT_GJ
eeprom_test
Diff: main.cpp
- Revision:
- 67:cfc88fd0591e
- Parent:
- 66:d385a08132d7
- Child:
- 68:135a2d087198
diff -r d385a08132d7 -r cfc88fd0591e main.cpp --- a/main.cpp Thu May 21 02:08:49 2020 +0000 +++ b/main.cpp Wed Jun 03 08:46:45 2020 +0000 @@ -186,8 +186,16 @@ // make_delay(); // CAN +// can.reset(); + CAN_ID_INIT(); +// can.filter(msg.id, 0xFFFFF000, CANStandard); +// can.frequency(1000000); // set CAN bit rate to 1Mbps + can.filter(CID_RX_CMD, 0xFFF, CANStandard, 0); + can.filter(CID_RX_REF_POSITION, 0xFFF, CANStandard, 1); + can.filter(CID_RX_REF_VALVE_POS, 0xFFF, CANStandard, 2); + can.filter(CID_RX_REF_PWM, 0xFFF, CANStandard, 3); can.attach(&CAN_RX_HANDLER); - CAN_ID_INIT(); + make_delay(); //Timer priority @@ -195,8 +203,7 @@ //NVIC_SetPriority(TIM2_IRQn, 3); NVIC_SetPriority(TIM4_IRQn, 3); - //can.reset(); - can.filter(msg.id, 0xFFFFF000, CANStandard); + // spi _ enc spi_enc_set_init(); @@ -425,7 +432,7 @@ //int raw_cur = ADC3->DR; //while((ADC3->SR & 0b10)); float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz - float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA + float cur_new = ((float)ADC3->DR-2048.0f) / 4096.0f * 3.3f * 10.0f / 1.44f; // unit : mA cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur); //cur.sen = raw_cur; @@ -500,36 +507,76 @@ } case MODE_TORQUE_SENSOR_NULLING: { - // DAC Voltage reference set + + if(SENSING_MODE == 0){ + + // DAC Voltage reference set + if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { + CUR_TORQUE_sum += torq.sen; + + if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { + CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f; + CUR_TORQUE_sum = 0; + + TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean); + + if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f; + if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f; + + //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); + dac_1 = TORQUE_VREF / 3.3f; + } + } else { + CONTROL_UTILITY_MODE = MODE_NO_ACT; + TMR3_COUNT_TORQUE_NULL = 0; + CUR_TORQUE_sum = 0; + CUR_TORQUE_mean = 0; + + ROM_RESET_DATA(); + + dac_1 = TORQUE_VREF / 3.3f; + } + } else { if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { - CUR_TORQUE_sum += torq.sen; + CUR_PRES_A_sum += pres_A.sen; + CUR_PRES_B_sum += pres_B.sen; if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { - CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f; - CUR_TORQUE_sum = 0; - - TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean); + 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; - if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f; - if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f; + float VREF_NullingGain = 0.0003f; + PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean; + PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean; - //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); - dac_1 = TORQUE_VREF / 3.3f; + 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_UTILITY_MODE = MODE_NO_ACT; TMR3_COUNT_TORQUE_NULL = 0; - CUR_TORQUE_sum = 0; - CUR_TORQUE_mean = 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 = TORQUE_VREF / 3.3f; - + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + //pc.printf("nulling end"); } - TMR3_COUNT_TORQUE_NULL++; - break; } + TMR3_COUNT_TORQUE_NULL++; + break; + } // case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { // if (TMR3_COUNT_DEADZONE == 0) { @@ -1604,17 +1651,17 @@ // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - if (SENSING_MODE == 0) { + //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)); - } + //} 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)); + //} } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - if (SENSING_MODE == 0) { + //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)); - } + //} 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)); + //} } } if (flag_data_request[1] == HIGH) { @@ -1643,7 +1690,7 @@ CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400 } - //If it doesn't rest, below can can not work. +// If it doesn't rest, below can can not work. for (can_rest = 0; can_rest < 10000; can_rest++) { ; }