![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 38:118df027d851
- Parent:
- 36:a46e63505ed8
- Child:
- 39:e961db3b93f8
- Child:
- 40:3f2c0619c8c4
diff -r 60a116154383 -r 118df027d851 main.cpp --- a/main.cpp Mon Nov 25 05:04:12 2019 +0000 +++ b/main.cpp Thu Nov 28 09:18:03 2019 +0000 @@ -206,15 +206,15 @@ make_delay(); //DAC init - dac_1 = PRES_A_VREF / 3.3; - dac_2 = PRES_B_VREF / 3.3; + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; make_delay(); for (int i=0; i<50; i++) { if(i%2==0) - ID_index_array[i] = - i * 0.5; + ID_index_array[i] = - i * 0.5f; else - ID_index_array[i] = (i+1) * 0.5; + ID_index_array[i] = (i+1) * 0.5f; } /************************************ @@ -246,9 +246,17 @@ 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 = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER; + if(JOINT_VEL[i+1] == JOINT_VEL[i]){ + Ref_Valve_Pos_FF = DDV_CENTER; + }else{ + 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 = ((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]); + if(JOINT_VEL[i+1] == JOINT_VEL[i-1]){ + Ref_Valve_Pos_FF = DDV_CENTER; + }else{ + 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; } @@ -271,6 +279,12 @@ { int i = 0; + if(REF_VALVE_POS > VALVE_MAX_POS) { + REF_VALVE_POS = VALVE_MAX_POS; + } else if(REF_VALVE_POS < VALVE_MIN_POS) { + REF_VALVE_POS = VALVE_MIN_POS; + } + valve_pos_err = REF_VALVE_POS - value; valve_pos_err_diff = valve_pos_err - valve_pos_err_old; valve_pos_err_old = valve_pos_err; @@ -280,11 +294,7 @@ VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff; - if(REF_VALVE_POS > VALVE_MAX_POS) { - REF_VALVE_POS = VALVE_MAX_POS; - } else if(REF_VALVE_POS < VALVE_MIN_POS) { - REF_VALVE_POS = VALVE_MIN_POS; - } + // if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0]) @@ -399,16 +409,17 @@ ADC1->CR2 |= 0x40000000; // adc _ 12bit //while((ADC1->SR & 0b10)); float alpha_update_pres_A = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f)); - 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_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 + //Pressure sensor 1B - //ADC2->CR2 |= 0x40000000; // adc _ 12bit - //while((ADC2->SR & 0b10)); - float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f)); - float pres_B_new = ((float)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR; - pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B); - torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B; + //float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f)); + //float pres_B_new = ((float)ADC2->DR); + //pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B); + //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B; @@ -526,19 +537,17 @@ // if(Ref_Joint_Vel_Act > 0) K_ff = 0.75f; // close // VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50f; - // torque feedback - // torq.err = torq.ref - torq.sen; - // torq.err_diff = torq.err - torq.err_old; - // torq.err_old = torq.err; - // torq.err_sum += torq.err; - // if (torq.err_sum > 1000) torq.err_sum = 1000; - // if (torq.err_sum<-1000) torq.err_sum = -1000; - // VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff; - // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f; - VALVE_POS_RAW_FORCE_FB = 0.0f; + //torque feedback + torq.err = - torq.ref + torq.sen; + torq.err_diff = torq.err - torq.err_old; + torq.err_old = torq.err; + torq.err_sum += torq.err; + if (torq.err_sum > 1000) torq.err_sum = 1000; + if (torq.err_sum<-1000) torq.err_sum = -1000; + 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_CENTER; if (valve_pos.ref > DDV_CENTER) { valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER; @@ -1116,9 +1125,9 @@ CUR_PRES_A_sum = 0; CUR_PRES_B_sum = 0; - float VREF_NullingGain = 0.003f; - PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean); - PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean); + 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; @@ -1685,8 +1694,8 @@ if (flag_data_request[2] == HIGH) { //pressure A and B - //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar) - CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) + CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF * 1000.0)); // CUR_PRES_X : 0(0bar)~4096(210bar) + //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.)); // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.); // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);