![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: main.cpp
- Revision:
- 55:b25725257569
- Parent:
- 54:647072f5307a
- Child:
- 56:6f50d9d3bfee
diff -r 647072f5307a -r b25725257569 main.cpp --- a/main.cpp Tue Feb 25 12:56:39 2020 +0000 +++ b/main.cpp Wed Feb 26 12:51:52 2020 +0000 @@ -136,7 +136,7 @@ /********************************* *** Initialization *********************************/ - //LED = 1; + LED = 1; //pc.baud(9600); // i2c init @@ -172,9 +172,9 @@ make_delay(); // TMR2 init -// Init_TMR2(); -// TIM2->CR1 ^= TIM_CR1_UDIS; -// make_delay(); + Init_TMR2(); + TIM2->CR1 ^= TIM_CR1_UDIS; + make_delay(); // CAN can.attach(&CAN_RX_HANDLER); @@ -183,7 +183,7 @@ //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); -// NVIC_SetPriority(TIM2_IRQn, 3); + NVIC_SetPriority(TIM2_IRQn, 3); NVIC_SetPriority(TIM4_IRQn, 3); //can.reset(); @@ -206,7 +206,7 @@ else ID_index_array[i] = (i+1) * 0.5f; } - + /************************************ *** Program is operating! *************************************/ @@ -234,15 +234,15 @@ 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) { if(JOINT_VEL[i+1] == JOINT_VEL[i]) { - Ref_Valve_Pos_FF = DDV_CENTER; + Ref_Valve_Pos_FF = (float) VALVE_CENTER; } else { - Ref_Valve_Pos_FF = ((float) 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])) + (float) VALVE_CENTER; } } else { if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) { - Ref_Valve_Pos_FF = DDV_CENTER; + Ref_Valve_Pos_FF = (float) VALVE_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]); + 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])) + (float) VALVE_CENTER + (float) (10*ID_index_array[i-1]); } } break; @@ -254,7 +254,7 @@ Ref_Valve_Pos_FF = (float) VALVE_MIN_POS; } - Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER); + Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER); return Ref_Valve_Pos_FF; } @@ -367,10 +367,10 @@ //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; + 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 @@ -393,11 +393,13 @@ float DT_TMR3 = (float)DT_5k; int cnt_trans = 0; double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f; -int canfreq = CAN_FREQUENCY; +int can_rest =0; extern "C" void TIM3_IRQHandler(void) { if (TIM3->SR & TIM_SR_UIF ) { + + if ((OPERATING_MODE&0b110) == 0) { K_v = 0.4f; // Moog (LPM >> mA) , 100bar @@ -591,7 +593,7 @@ pos.err = pos.ref_home_pos - pos.sen; float VALVE_POS_RAW_POS_FB = 0.0f; VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f; - valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER; + valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; VALVE_POS_CONTROL(valve_pos.ref); //float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control @@ -621,7 +623,7 @@ pos.err = pos.ref_home_pos - (float)pos.sen; float VALVE_POS_RAW_POS_FB = 0.0f; VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION; - valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER; + valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; VALVE_POS_CONTROL(valve_pos.ref); cnt_findhome++; @@ -982,7 +984,7 @@ DZ_index= DZ_index *2; if(DZ_index >= 128) { SECOND_DZ = valve_pos.ref; - DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ); + VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; VALVE_DEADZONE_PLUS = (float) SECOND_DZ; @@ -1067,7 +1069,7 @@ DZ_index= DZ_index *2; if(DZ_index >= 128) { SECOND_DZ = valve_pos.ref; - DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ); + VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; VALVE_DEADZONE_PLUS = (float) SECOND_DZ; @@ -1099,7 +1101,7 @@ pos_minus_end = pos.sen; first_check = 1; VALVE_FR_timer = 0; - valve_pos.ref = DDV_CENTER; + valve_pos.ref = (float) VALVE_CENTER; ID_index = 0; max_check = 0; min_check = 0; @@ -1109,12 +1111,12 @@ V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION; } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { data_num = 0; - valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER; + valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_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.0f*((float) ID_index_array[ID_index]) + DDV_CENTER; + valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_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) { @@ -1276,7 +1278,7 @@ double KI_I = 0.1f * R_model*w0; // KNR Valve Current Control Gain - if ((OPERATING_MODE & 0x02) == 1) { // KNR Valve + if ((OPERATING_MODE & 0b110) == 1) { // KNR Valve R_model = 163.0f; // ohm L_model = 1.0f; w0 = 2.0f * 3.14f * 80.0f; @@ -1319,7 +1321,7 @@ if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER * mV_PER_pulse; // unit: mV + VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV } else { VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; @@ -1364,19 +1366,17 @@ TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { - - if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/canfreq) == 0) { -// if (TMR2_COUNT_CAN_TX % 10 == 0) { // Position, Velocity, and Torque (ID:1200) - if (flag_data_request[0] == HIGH) { - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator + if (flag_data_request[0] == LOW) { + 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)); } 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)); } - } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator + } 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)); } else if (SENSING_MODE == 1) { @@ -1384,30 +1384,38 @@ } } } - if (flag_data_request[1] == HIGH) { + if (flag_data_request[1] == LOW) { //valve position double t_value = 0; - if(value>=DDV_CENTER) { - t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER); + if(value>=(float) 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)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER); + t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); } - CAN_TX_TORQUE((int16_t) (t_value)); + CAN_TX_TORQUE((int16_t) (t_value)); //1300 } - - if (flag_data_request[2] == HIGH) { + + + 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) + CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400 } - - if (flag_data_request[3] == HIGH) { + + //If it doesn't rest, below can can not work. + for (can_rest = 0; can_rest < 10000; can_rest++) { + ; + } + + if (flag_data_request[3] == LOW) { //PWM - CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS); + CAN_TX_PWM((int16_t) cur.sen); //1500 } - - if (flag_data_request[4] == HIGH) { + //for (i = 0; i < 10000; i++) { +// ; +// } + if (flag_data_request[4] == LOW) { //valve position - 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) (CAN_FREQ), (int16_t) (D_DAMPER), (int16_t) OPERATING_MODE); //1600 } // Others : Reference position, Reference FT, PWM, Current (ID:1300) @@ -1422,8 +1430,6 @@ } TMR2_COUNT_CAN_TX++; - - } TIM3->SR = 0x0; // reset the status register @@ -1434,58 +1440,63 @@ //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] == HIGH) { +// //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] == HIGH) { +// //if (flag_data_request[1] == LOW) { // //valve position // double t_value = 0; -// if(value>=DDV_CENTER) { -// t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER); +// 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)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER); +// 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] == HIGH) { +// //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) -// //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.); +// +// //} // +// //if (flag_data_request[3] == LOW) { +// //PWM +// int i = 0; +// for (i = 0; i < 10000; i++) { +// ; // } -// -// if (flag_data_request[3] == HIGH) { -// //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.)); // -// } +// //} +// +// //if (flag_data_request[4] == LOW) { +// //valve position // -// if (flag_data_request[4] == HIGH) { -// //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) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out); +// //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); -// } +// //} // // // }