Sungwoo Kim
/
HydraulicControlBoard_LIGHT
For LIGHT
Revision 67:cfc88fd0591e, committed 2020-06-03
- Comitter:
- Lightvalve
- Date:
- Wed Jun 03 08:46:45 2020 +0000
- Parent:
- 66:d385a08132d7
- Commit message:
- new ankle
Changed in this revision
diff -r d385a08132d7 -r cfc88fd0591e CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Thu May 21 02:08:49 2020 +0000 +++ b/CAN/function_CAN.cpp Wed Jun 03 08:46:45 2020 +0000 @@ -698,7 +698,7 @@ void CAN_RX_HANDLER() { - + CAN_TX_PWM((int16_t) 20); //1500 can.read(msg); unsigned int address = msg.id; if(address==CID_RX_CMD){ @@ -736,6 +736,7 @@ valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f; } } + } else if(address==CID_RX_REF_PWM){ int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8); Vout.ref = (double) temp_ref_pwm;
diff -r d385a08132d7 -r cfc88fd0591e function_utilities/function_utilities.cpp --- a/function_utilities/function_utilities.cpp Thu May 21 02:08:49 2020 +0000 +++ b/function_utilities/function_utilities.cpp Wed Jun 03 08:46:45 2020 +0000 @@ -376,7 +376,7 @@ { FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector if (!writer.ready()) writer.open(); -// BNO = 10; +// BNO = 11; writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀 writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE); writer.write(RID_SENSING_MODE, (int) SENSING_MODE); @@ -457,9 +457,10 @@ void ROM_CALL_DATA(void) { BNO = flashReadInt(Rom_Sector, RID_BNO); - //BNO = 11; +// BNO = 11; OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE); SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE); + SENSING_MODE = 1; CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE); CURRENT_CONTROL_MODE = 1; FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE); @@ -505,9 +506,9 @@ //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle // TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f; -// PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; + PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f; PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f; -// PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; + PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f; FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f; HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET); HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
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++) { ; }