![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 48:889798ff9329
- Parent:
- 47:fdcb8bd86fd6
- Child:
- 49:e7bcfc244d40
--- a/main.cpp Tue Jan 21 13:39:27 2020 +0000 +++ b/main.cpp Wed Jan 29 13:50:24 2020 +0000 @@ -73,7 +73,6 @@ int timer_while = 0; int while_index = 0; - extern int CID_RX_CMD; extern int CID_RX_REF_POSITION; extern int CID_RX_REF_PWM; @@ -240,7 +239,7 @@ { int i = 0; - float Ref_Valve_Pos_FF = DDV_CENTER; + float Ref_Valve_Pos_FF = 0.0f; 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) { @@ -266,8 +265,9 @@ } Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER); - +// Ref_Valve_Pos_FF = 0.0; return Ref_Valve_Pos_FF; + } @@ -323,9 +323,9 @@ for(i=0; i<24; i++) { if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { if(i==0) { - VALVE_PWM_RAW_FF = (float) 1000.0 / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i]); + VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i]); } else { - VALVE_PWM_RAW_FF = (float) 1000.0* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0* (float) ID_index_array[i-1]; + VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1]; } break; } @@ -442,26 +442,28 @@ //unsigned long CNT_TMR3 = 0; //float FREQ_TMR3 = (float)FREQ_5k; float FREQ_TMR3 = (float)FREQ_1k; -//float DT_TMR3 = (float)DT_5k; -float DT_TMR3 = (float)DT_1k; +float DT_TMR3 = (float)DT_5k; +//float DT_TMR3 = (float)DT_1k; int cnt_trans = 0; +double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f; + extern "C" void TIM3_IRQHandler(void) { if (TIM3->SR & TIM_SR_UIF ) { ENC_UPDATE(); if(MODE_POS_FT_TRANS == 1){ - alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f; + alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; cnt_trans++; torq.err_sum = 0; - if(cnt_trans>3*TMR_FREQ_5k) + if((float)cnt_trans * DT_TMR3 > 3.0f) MODE_POS_FT_TRANS = 2; } - if(MODE_POS_FT_TRANS == 3){ - alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f; + else if(MODE_POS_FT_TRANS == 3){ + alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; cnt_trans++; torq.err_sum = 0; - if(cnt_trans>3*TMR_FREQ_5k) + if((float) cnt_trans * DT_TMR3 > 3.0f ) MODE_POS_FT_TRANS = 0; } else if(MODE_POS_FT_TRANS == 2){ @@ -494,18 +496,18 @@ pos.err = pos.ref - pos.sen; //[pulse] vel.err = vel.ref - vel.sen; //[pulse/s] - //double K_spring = 1.0f; //[N/mm] - //double D_damper = 0.0001f; - torq.ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N] + double torq_ref = 0.0f; + torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N] // torque feedback - torq.err = torq.ref - torq.sen; //[pulse] + torq.err = torq_ref - torq.sen; //[pulse] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse] if (torq.err_sum > 1000) torq.err_sum = 1000; if (torq.err_sum<-1000) torq.err_sum = -1000; - VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) / (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION * 0.01f + DDV_JOINT_POS_FF(vel.ref)); + VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f + + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + DDV_JOINT_POS_FF(vel.ref)); if (VALVE_POS_RAW_FORCE_FB >= 0) { valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS; @@ -556,7 +558,8 @@ VALVE_POS_RAW_FORCE_FB = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff; VALVE_POS_RAW_FORCE_FB = VALVE_POS_RAW_FORCE_FB * 0.01f; - 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_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; + valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref); if (valve_pos.ref >= 0) { valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS; @@ -1090,7 +1093,7 @@ pos.ref_home_pos = 0.0f; vel.ref_home_pos = 0.0f; FINDHOME_STAGE = FINDHOME_INIT; - CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION; + CONTROL_MODE = MODE_JOINT_CONTROL; } } @@ -1323,14 +1326,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); DDV_POS_AVG = VALVE_POS_TMP / data_num; START_POS = pos.sen; - //CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num)); VALVE_POS_TMP = 0; data_num = 0; } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) { valve_pos.ref = DDV_POS_AVG; VALVE_POS_CONTROL(valve_pos.ref); - //CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS)); } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) { valve_pos.ref = DDV_POS_AVG; @@ -1343,18 +1344,6 @@ valve_pos.ref = DDV_POS_AVG; VALVE_POS_CONTROL(valve_pos.ref); FINAL_POS = pos.sen; - // if(VEL_POINT >= 0) - // { - // DZ_case = 1; - // } - // else - // { - // DZ_case = -1; - // } - // if(abs(FINAL_POS - START_POS) < 100) - // { - // DZ_case = 0; - // } if((FINAL_POS - START_POS)>100) { DZ_case = 1; @@ -1371,7 +1360,6 @@ Ref_Valve_Pos_Old = DDV_POS_AVG; DZ_NUM = 1; DZ_index = 1; - //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM)); } } else { @@ -1395,12 +1383,6 @@ } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; - // if(VEL_POINT >= 0) - // DZ_DIRECTION = 1 * DZ_case; - // else - // DZ_DIRECTION = -1 * DZ_case; - // if(abs(FINAL_POS - START_POS) < 100) - // DZ_DIRECTION = 1 * DZ_case; if((FINAL_POS - START_POS)>100) { DZ_DIRECTION = 1 * DZ_case; @@ -1421,8 +1403,6 @@ DZ_index = 1; DZ_DIRECTION = 1; } - - //CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index)); } } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { @@ -1444,12 +1424,6 @@ } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; - // if(VEL_POINT >= 0) - // DZ_DIRECTION = 1 * DZ_case; - // else - // DZ_DIRECTION = -1 * DZ_case; - // if(abs(FINAL_POS - START_POS) < 100) - // DZ_DIRECTION = -1 * DZ_case; if((FINAL_POS - START_POS)>100) { DZ_DIRECTION = -1; @@ -1476,8 +1450,6 @@ CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } - - //CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index)); } } else if(DZ_case == 0 && DZ_NUM ==1) { if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { @@ -1499,12 +1471,6 @@ } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; - // if(VEL_POINT >= 0) - // DZ_DIRECTION = 1; - // else - // DZ_DIRECTION = -1; - // if(abs(FINAL_POS - START_POS) < 100) - // DZ_DIRECTION = 1; if((FINAL_POS - START_POS)>100) { DZ_DIRECTION = 1; @@ -1515,7 +1481,6 @@ } VEL_POINT = 0; - //CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION)); VALVE_DZ_timer = 0; DZ_index= DZ_index *2; if(DZ_index >= 128) { @@ -1557,7 +1522,6 @@ VALVE_DZ_timer = 0; - //CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index)); DZ_index= DZ_index *2; if(DZ_index >= 128) { SECOND_DZ = valve_pos.ref; @@ -1569,8 +1533,6 @@ CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } - - //CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index)); } } } @@ -1631,25 +1593,19 @@ JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec //ROM_RESET_DATA(); - - //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); - //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF)); VALVE_FR_timer = 0; one_period_end = 0; ID_index= ID_index +1; - // CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index)); V_out = 0.0f; } if(max_check == 1 && min_check == 1) { - //spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index)); VALVE_POS_NUM = ID_index; //ROM_RESET_DATA(); ID_index = 0; first_check = 0; VALVE_FR_timer = 0; - // CAN_TX_PRES((int16_t) (123), (int16_t) (123)); CONTROL_MODE = MODE_NO_ACT; } } @@ -1741,8 +1697,13 @@ //torque //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.)); //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM)); - CAN_TX_TORQUE((int16_t) (cur.sen)); - // CAN_TX_TORQUE((int16_t) DZ_temp_cnt); + double t_value = 0; + if(value>=DDV_CENTER){ + t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER); + }else{ + t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER); + } + CAN_TX_TORQUE((int16_t) (t_value)); } if (flag_data_request[2] == HIGH) { @@ -1757,7 +1718,7 @@ if (flag_data_request[3] == HIGH) { //PWM - CAN_TX_PWM((int16_t) CUR_PWM); + CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS); // CAN_TX_PWM((int16_t) cnt_vel_findhome); // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.)); // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.)); @@ -1766,7 +1727,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); + CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING); //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)); @@ -1775,6 +1736,7 @@ // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos); // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate); } + } TIM2->SR = 0x0; // reset the status register