![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 33:91b17819ec30
- Parent:
- 32:4b8c0fedaf2c
- Child:
- 34:bb2ca2fc2a8e
--- a/main.cpp Mon Nov 11 09:48:00 2019 +0000 +++ b/main.cpp Tue Nov 12 11:29:36 2019 +0000 @@ -238,28 +238,28 @@ } } -int DDV_JOINT_POS_FF(float REF_JOINT_VEL) +float DDV_JOINT_POS_FF(float REF_JOINT_VEL) { int i = 0; - int Ref_Valve_Pos_FF = DDV_CENTER; + float Ref_Valve_Pos_FF = DDV_CENTER; for(i=0; i<VALVE_POS_NUM; i++) { if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) { if(i==0) { - Ref_Valve_Pos_FF = ((int) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER; + Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER; } else { - Ref_Valve_Pos_FF = ((int) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 10*ID_index_array[i-1]; + Ref_Valve_Pos_FF = ((float) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + (float) (10*ID_index_array[i-1]); } break; } } if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { - Ref_Valve_Pos_FF = VALVE_MAX_POS; + Ref_Valve_Pos_FF = (float) VALVE_MAX_POS; } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { - Ref_Valve_Pos_FF = VALVE_MIN_POS; + Ref_Valve_Pos_FF = (float) VALVE_MIN_POS; } - Ref_Valve_Pos_FF = (int) ( (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + (float) DDV_CENTER); + Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER; return Ref_Valve_Pos_FF; } @@ -535,7 +535,7 @@ // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f; VALVE_POS_RAW_FORCE_FB = 0.0f; - valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB + VALVE_CENTER_OFFSET; // valve center offset = - 2.0f; + valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER; if (valve_pos.ref > DDV_CENTER) @@ -1402,17 +1402,13 @@ DZ_index= DZ_index *2; if(DZ_index >= 128) { SECOND_DZ = valve_pos.ref; - DDV_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); + DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ); first_check = 0; - VALVE_DEADZONE_MINUS = FIRST_DZ; - VALVE_DEADZONE_PLUS = SECOND_DZ; + VALVE_DEADZONE_MINUS = (float) FIRST_DZ; + VALVE_DEADZONE_PLUS = (float) SECOND_DZ; ROM_RESET_DATA(); - - //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); - //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); - //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER); - //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); + CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } @@ -1501,17 +1497,11 @@ DZ_index= DZ_index *2; if(DZ_index >= 128) { SECOND_DZ = valve_pos.ref; - DDV_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); + DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ); first_check = 0; - VALVE_DEADZONE_MINUS = FIRST_DZ; - //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); - VALVE_DEADZONE_PLUS = SECOND_DZ; + VALVE_DEADZONE_MINUS = (float) FIRST_DZ; + VALVE_DEADZONE_PLUS = (float) SECOND_DZ; - //ROM_RESET_DATA(); - - //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); - //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER); - //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } @@ -1551,12 +1541,12 @@ V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { data_num = 0; - valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER; + valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER; VALVE_POS_CONTROL(valve_pos.ref); START_POS = pos.sen; } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) { - valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER; + valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER; VALVE_POS_CONTROL(valve_pos.ref); data_num = data_num + 1; if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) { @@ -1717,9 +1707,7 @@ if (flag_data_request[4] == HIGH) { //valve position CAN_TX_VALVE_POSITION((int16_t) (value), (int16_t) valve_pos.ref, (int16_t) V_out); - //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); - //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF)); - + //CAN_TX_VALVE_POSITION((int16_t) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out); //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM)); // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));