![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 23:59218d4a256d
- Parent:
- 21:e5f1a43ea6f9
- Child:
- 24:ef6e1092e9e6
diff -r 966b35691d69 -r 59218d4a256d main.cpp --- a/main.cpp Mon Sep 09 10:41:16 2019 +0000 +++ b/main.cpp Tue Sep 10 09:38:26 2019 +0000 @@ -181,16 +181,23 @@ make_delay(); // TMR5 init - Init_TMR5(); - TIM5->CR1 ^= TIM_CR1_UDIS; + Init_TMR2(); + TIM2->CR1 ^= TIM_CR1_UDIS; make_delay(); // CAN can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); make_delay(); + + //Timer priority + NVIC_SetPriority(TIM3_IRQn, 2); + NVIC_SetPriority(TIM2_IRQn, 3); + NVIC_SetPriority(TIM4_IRQn, 4); + + //can.reset(); can.filter(msg.id, 0xFFFFF000, CANStandard); - + // spi _ enc spi_enc_set_init(); make_delay(); @@ -270,6 +277,34 @@ } else if(REF_VALVE_POS < VALVE_MIN_POS) { REF_VALVE_POS = VALVE_MIN_POS; } + + +// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0]) +// { +// if(REF_VALVE_POS <= VALVE_POS_VS_PWM[1]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[1] - VALVE_POS_VS_PWM[0]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[0]); +// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[3]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[3] - VALVE_POS_VS_PWM[1]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[1]) + 3000.0* (double) ID_index_array[1]; +// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[5]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[5] - VALVE_POS_VS_PWM[3]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[3]) + 3000.0* (double) ID_index_array[3]; +// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[7]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[7] - VALVE_POS_VS_PWM[5]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[5]) + 3000.0* (double) ID_index_array[5]; +// }else +// VALVE_PWM_RAW_FF = 12000.0; +// } +// else +// { +// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[2]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[0] - VALVE_POS_VS_PWM[2]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[2]) + 3000.0* (double) ID_index_array[2]; +// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[4]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[2] - VALVE_POS_VS_PWM[4]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[4]) + 3000.0* (double) ID_index_array[4]; +// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[6]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[4] - VALVE_POS_VS_PWM[6]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[6]) + 3000.0* (double) ID_index_array[6]; +// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[8]) { +// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[6] - VALVE_POS_VS_PWM[8]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[8]) + 3000.0* (double) ID_index_array[8]; +// }else +// VALVE_PWM_RAW_FF = -12000.0; +// } 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])) { @@ -334,6 +369,7 @@ double DT_TMR4 = (double)DT_10k; extern "C" void TIM4_IRQHandler(void) { + if (TIM4->SR & TIM_SR_UIF ) { /******************************************************* @@ -388,8 +424,10 @@ int j =0; //unsigned long CNT_TMR3 = 0; -double FREQ_TMR3 = (double)FREQ_5k; -double DT_TMR3 = (double)DT_5k; +//double FREQ_TMR3 = (double)FREQ_5k; +double FREQ_TMR3 = (double)FREQ_1k; +//double DT_TMR3 = (double)DT_5k; +double DT_TMR3 = (double)DT_1k; extern "C" void TIM3_IRQHandler(void) { if (TIM3->SR & TIM_SR_UIF ) { @@ -454,6 +492,8 @@ } case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: { + + double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback //double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback @@ -488,9 +528,11 @@ // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01; VALVE_POS_RAW_FORCE_FB = 0.0; - //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; + 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; VALVE_POS_CONTROL(valve_pos.ref); + + break; } @@ -608,7 +650,7 @@ // I_REF = (2.0 * sin(2. * 2. * 3.14 * (double) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))); if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { - CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas + //CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas } break; } @@ -629,7 +671,7 @@ } if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { - CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas + //CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas } break; } @@ -1166,10 +1208,6 @@ VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; for(i=0; i<25; i++) { VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); - - //ROM_RESET_DATA(); - - //spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i])); if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) { VALVE_MAX_POS = VALVE_POS_AVG[i]; VALVE_POS_AVG_OLD = VALVE_MAX_POS; @@ -1178,12 +1216,7 @@ VALVE_POS_AVG_OLD = VALVE_MIN_POS; } } - ROM_RESET_DATA(); - - //spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS)); - //spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS)); - CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS)); ID_index = 0; CONTROL_MODE = MODE_NO_ACT; } @@ -1216,7 +1249,7 @@ V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) 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)); + //CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num)); VALVE_POS_TMP = 0; data_num = 0; @@ -1374,7 +1407,7 @@ DZ_index = 1; } - CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index)); + //CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index)); } } else if(DZ_case == 0 && DZ_NUM ==1) { if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { @@ -1467,7 +1500,7 @@ //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); + //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } @@ -1484,7 +1517,7 @@ if(first_check == 0) { if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { V_out = VALVE_VOLTAGE_LIMIT; - CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); + //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { V_out = VALVE_VOLTAGE_LIMIT; pos_plus_end = pos.sen; @@ -1588,88 +1621,6 @@ V_out = V_out; } - //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE); - //PWM_out = CUR_PWM; - - /* - - //CAN ---------------------------------------------------------------------- - //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { - if (TMR3_COUNT_CAN_TX % 1000 == 0) { - - if (flag_data_request[0] == HIGH) { - //position+velocity - CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); - //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0); - //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]); - //pc.printf("can good"); - // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR); - } - - if (flag_data_request[1] == HIGH) { - //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) (Ref_Valve_Pos_FF_CAN)); - // CAN_TX_TORQUE((int16_t) DZ_temp_cnt); - } - - 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) (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] == HIGH) { - //PWM - CAN_TX_PWM((int16_t) CUR_PWM); - //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN)); - // 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] == HIGH) { - //valve position - CAN_TX_VALVE_POSITION((int16_t) (value)); - //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) (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); - } - - TMR3_COUNT_CAN_TX = 0; - - } - TMR3_COUNT_CAN_TX++; - - */ - - - /******************************************************* - *** Valve Control - ********************************************************/ - //ValveControl(CONTROL_MODE); - -// double t = (double)CNT_TMR4*DT_TMR4; -// double T = 1.0; -// V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty) -// if(V_out > 0.0) V_out = 1000.0; -// else if(V_out < 0.0) V_out = -1000.0; - - //V_out = 2000.0 * sin(2 * 3.14159 * (double) CNT_TMR3 / (double) 5000.0); - /******************************************************* *** PWM ********************************************************/ @@ -1695,37 +1646,6 @@ TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v); TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w); - /******************************************************* - *** Data Send (CAN) & Print out (UART) - ********************************************************/ -// if((CNT_TMR3%1000)==0){ -// msg.id = 50; -// msg.len = 4; -// int temp_CUR = (int)(cur.sen*1000.0); -// msg.data[0]=0x00FF&temp_CUR; -// msg.data[1]=0x00FF&(temp_CUR>>8); -// int temp_PWM = (int)(V_out); -// msg.data[2]=0x00FF&temp_PWM; -// msg.data[3]=0x00FF&(temp_PWM>>8); -// can.write(msg); -// } - - //if((CNT_TMR3%5000)==0) { -// if(LED==1) { -// LED=0; -// } else -// LED = 1; -// LED != LED; -// pc.printf("A %f\n", (double) pres_A.sen); -// CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[j]); -// j++; - //} - - /******************************************************* - *** Timer Counting & etc. - ********************************************************/ - //CNT_TMR3++; - } TIM3->SR = 0x0; // reset the status register @@ -1736,9 +1656,9 @@ //unsigned long CNT_TMR5 = 0; //double FREQ_TMR5 = (double)FREQ_500; //double DT_TMR5 = (double)DT_500; -extern "C" void TIM5_IRQHandler(void) +extern "C" void TIM2_IRQHandler(void) { - if (TIM5->SR & TIM_SR_UIF ) { + if (TIM2->SR & TIM_SR_UIF ) { //CAN ---------------------------------------------------------------------- if (flag_data_request[0] == HIGH) { @@ -1794,7 +1714,7 @@ } } - TIM5->SR = 0x0; // reset the status register + TIM2->SR = 0x0; // reset the status register } @@ -1831,3 +1751,4 @@ cur.err_int = cur.err_int - V_rem*DT_5k; } } +