![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 56:2eade98630e2
- Parent:
- 55:f4819de54e7a
- Child:
- 57:f308b1656d9c
--- a/main.cpp Fri Mar 06 12:14:38 2020 +0000 +++ b/main.cpp Mon Mar 09 12:43:04 2020 +0000 @@ -194,10 +194,13 @@ make_delay(); //DAC init -// dac_1 = PRES_A_VREF / 3.3f; -// dac_2 = PRES_B_VREF / 3.3f; - dac_1 = TORQUE_VREF / 3.3f; - dac_2 = 0.0f; + if (SENSING_MODE == 0) { + dac_1 = TORQUE_VREF / 3.3f; + dac_2 = 0.0f; + } else if (SENSING_MODE == 1) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } make_delay(); for (int i=0; i<50; i++) { @@ -206,7 +209,7 @@ else ID_index_array[i] = (i+1) * 0.5f; } - + /************************************ *** Program is operating! *************************************/ @@ -356,21 +359,46 @@ if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) { ENC_UPDATE(); } + + ADC1->CR2 |= 0x40000000; + if (SENSING_MODE == 0) { + // Torque Sensing (0~210)bar ============================================= + float pres_A_new = (((float) ADC1->DR) - 2047.5f); + double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz + pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new; + torq.sen = pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE; - //Pressure sensor A - ADC1->CR2 |= 0x40000000; // adc _ 12bit - //while((ADC1->SR & 0b10)); - float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f)); - 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); //pulse -2047~2047 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later. + } else if (SENSING_MODE == 1) { + // Pressure Sensing (0~210)bar ============================================= + float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); + float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); + double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 500Hz + pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new; + pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new; + CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR; + CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR; - - //Pressure sensor B - float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.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; + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator + torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm + } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator + torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N + } + } + +// //Pressure sensor A +// ADC1->CR2 |= 0x40000000; // adc _ 12bit +// //while((ADC1->SR & 0b10)); +// float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f)); +// 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); //pulse -2047~2047 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later. +// +// +// //Pressure sensor B +// float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.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; //Current @@ -381,7 +409,7 @@ float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur); //cur.sen = raw_cur; - + CNT_TMR4++; } TIM4->SR = 0x0; // reset the status register @@ -398,8 +426,6 @@ extern "C" void TIM3_IRQHandler(void) { if (TIM3->SR & TIM_SR_UIF ) { - - if (((OPERATING_MODE&0b110)>>1) == 0) { K_v = 0.4f; // Moog (LPM >> mA) , 100bar @@ -426,17 +452,17 @@ if((float) cnt_trans * DT_TMR3 > 3.0f ) MODE_POS_FT_TRANS = 0; } else if(MODE_POS_FT_TRANS == 2) { - alpha_trans = 1.0; + alpha_trans = 1.0f; cnt_trans = 0; } else { - alpha_trans = 0.0; + alpha_trans = 0.0f; cnt_trans = 0; } int UTILITY_MODE = 0; int CONTROL_MODE = 0; - + if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) { UTILITY_MODE = CONTROL_UTILITY_MODE; CONTROL_MODE = MODE_NO_ACT; @@ -472,7 +498,7 @@ dac_1 = TORQUE_VREF / 3.3f; } } else { - CONTROL_MODE = MODE_NO_ACT; + CONTROL_UTILITY_MODE = MODE_NO_ACT; TMR3_COUNT_TORQUE_NULL = 0; CUR_TORQUE_sum = 0; CUR_TORQUE_mean = 0; @@ -713,7 +739,6 @@ // } // // } - case MODE_PRESSURE_SENSOR_NULLING: { // DAC Voltage reference set if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) { @@ -727,8 +752,8 @@ CUR_PRES_B_sum = 0; 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); + PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean; + PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * 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; @@ -1156,7 +1181,7 @@ } break; } - + case MODE_SYSTEM_ID: { freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.; valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3); @@ -1168,8 +1193,8 @@ } break; } - - + + default: break; @@ -1191,7 +1216,7 @@ } else { //SW Valve VALVE_POS_CONTROL(valve_pos.ref); } - + break; } @@ -1203,11 +1228,11 @@ 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)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE); //[N] + torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE) - torq.sen; //[N] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { - + double I_REF_POS = 0.0f; double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback double I_REF_VC = 0.0f; // I_REF for velocity compensation @@ -1215,7 +1240,7 @@ double temp_vel_pos = 0.0f; double temp_vel_torq = 0.0f; double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control - + if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s // L when P-gain = 100, f_cut = 10Hz L feedforward velocity @@ -1275,10 +1300,10 @@ VALVE_POS_CONTROL(valve_pos.ref); } - + break; } - + case MODE_VALVE_OPEN_LOOP: { V_out = (float) Vout.ref; break; @@ -1290,7 +1315,7 @@ if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve - + //////////////////////////////////////////////////////////////////////////// //////////////////////////// CURRENT CONTROL ////////////////////////////// //////////////////////////////////////////////////////////////////////////// @@ -1368,8 +1393,8 @@ else if (CUR_PWM_lin < 0) V_out = (int) CUR_PWM_lin - 140; else V_out = CUR_PWM_lin; } - - + + /******************************************************* *** PWM @@ -1404,15 +1429,15 @@ if (flag_data_request[0] == HIGH) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.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*10.0f), (int16_t) (pres_B.sen*10.0f)); + 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) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f)); } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f)); + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); } } } @@ -1424,27 +1449,27 @@ // } else { // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); // } - if(OPERATING_MODE==5){ + if(OPERATING_MODE==5) { t_value = (double) value; - }else if(CURRENT_CONTROL_MODE==1){ + } else if(CURRENT_CONTROL_MODE==1) { t_value = cur.sen; - }else{ + } else { t_value = V_out; } CAN_TX_TORQUE((int16_t) (t_value)); //1300 } - - + + if (flag_data_request[2] == HIGH) { //pressure A and B 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. for (can_rest = 0; can_rest < 10000; can_rest++) { ; } - + if (flag_data_request[3] == HIGH) { //PWM CAN_TX_PWM((int16_t) cur.sen); //1500 @@ -1462,7 +1487,7 @@ // CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3)); // } //if (flag_delay_test == 1){ - //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref); + //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref); //} TMR2_COUNT_CAN_TX = 0; @@ -1472,115 +1497,4 @@ } TIM3->SR = 0x0; // reset the status register -} - - - -//unsigned long CNT_TMR5 = 0; -//float FREQ_TMR5 = (float)FREQ_500; -//float DT_TMR5 = (float)DT_500; - -//extern "C" void TIM2_IRQHandler(void) -//{ -// LED = 1; -// if (TIM2->SR & TIM_SR_UIF ) { -// -// //CAN ---------------------------------------------------------------------- -// //if (flag_data_request[0] == LOW) { -// //position+velocity -// CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f)); -// //} -// -// //if (flag_data_request[1] == LOW) { -// //valve position -// double t_value = 0; -// if(value>=(double)VALVE_CENTER) { -// t_value = 10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER); -// } else { -// t_value = -10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER); -// } -// CAN_TX_TORQUE((int16_t) (t_value)); -// //} -// -// //if (flag_data_request[2] == LOW) { -// //pressure A and B -// CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar) -// -// //} -// -// //if (flag_data_request[3] == LOW) { -// //PWM -// int i = 0; -// for (i = 0; i < 10000; i++) { -// ; -// } -// 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.)); -// -// //} -// -// //if (flag_data_request[4] == LOW) { -// //valve position -// -// for (i = 0; i < 10000; i++) { -// ; -// } -// 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) ((float) VALVE_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)); -// // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION); -// // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos); -// // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate); -// //} -// -// -// } -// TIM2->SR = 0x0; // reset the status register -//} - - -// -//void CurrentControl() -//{ -// cur.err = cur.ref - cur.sen; -// cur.err_int = cur.err_int + cur.err*DT_TMR4; -// cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4; -// cur.err_old = cur.err; -// -// float R_model = 150.0f; // ohm -// float L_model = 0.3f; -// float w0 = 2.0f*3.14f*90.0f; -// float KP_I = L_model*w0; -// float KI_I = R_model*w0; -// float KD_I = 0.0f; -// -// float FF_gain = 0.0f; -// V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff); -// // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV -// V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV -// -// float Ka = 5.0f/KP_I; -// if(V_out > V_MAX) { -// V_rem = V_out-V_MAX; -// V_rem = Ka*V_rem; -// V_out = V_MAX; -// cur.err_int = cur.err_int - V_rem*DT_5k; -// } else if(V_out < -V_MAX) { -// V_rem = V_out-(-V_MAX); -// V_rem = Ka*V_rem; -// V_out = -V_MAX; -// cur.err_int = cur.err_int - V_rem*DT_5k; -// } -//} - - - - - - - - +} \ No newline at end of file