Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 253:c53d4c26b04a, committed 2022-07-26
- Comitter:
- Lightvalve
- Date:
- Tue Jul 26 06:58:25 2022 +0000
- Parent:
- 252:3ffd78cbd029
- Child:
- 254:9f487eaa87b5
- Commit message:
- 220726
Changed in this revision
INIT_HW/INIT_HW.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/INIT_HW/INIT_HW.cpp Wed Jul 20 07:50:48 2022 +0000 +++ b/INIT_HW/INIT_HW.cpp Tue Jul 26 06:58:25 2022 +0000 @@ -139,50 +139,85 @@ void Init_TMR3() { + ///////////////Trigger by TIMER 1///////////////////////////////// + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // enable TIM3 clock NVIC_EnableIRQ(TIM3_IRQn); //Enable TIM3 IRQ TIM3->DIER |= TIM_DIER_UIE; // enable update interrupt - TIM3->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode + TIM3->CR1 = 0x10; TIM3->CR1 |= TIM_CR1_UDIS; - TIM3->RCR |= 0x001; // update event once per up/down count of TIM3 TIM3->EGR |= TIM_EGR_UG; - TIM3->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock - TIM3->ARR = TMR3_COUNT-1; // set auto reload, 5 khz + TIM3->PSC = 1-1; // 10kHz + TIM3->ARR = 1; TIM3->CNT = 0; TIM3->SR = 0; - TIM3->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. - TIM3->CR1 |= TIM_CR1_CEN; // enable TIM4 + TIM3->SMCR = 0x07; //External clock mode + TIM3->CR1 |= TIM_CR1_CEN; // enable TIM3 + +// RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // enable TIM3 clock +// NVIC_EnableIRQ(TIM3_IRQn); //Enable TIM3 IRQ +// +// TIM3->DIER |= TIM_DIER_UIE; // enable update interrupt +// TIM3->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode +// TIM3->CR1 |= TIM_CR1_UDIS; +// TIM3->RCR |= 0x001; // update event once per up/down count of TIM3 +// TIM3->EGR |= TIM_EGR_UG; +// +// TIM3->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock +// TIM3->ARR = TMR3_COUNT-1; // set auto reload, 5 khz +// TIM3->CNT = 0; +// TIM3->SR = 0; +// TIM3->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. +// TIM3->CR1 |= TIM_CR1_CEN; // enable TIM4 } void Init_TMR2() { + ///////////////Trigger by TIMER 1///////////////////////////////// + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // enable TIM2 clock - - //ISR Setup NVIC_EnableIRQ(TIM2_IRQn); //Enable TIM2 IRQ TIM2->DIER |= TIM_DIER_UIE; // enable update interrupt - TIM2->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode + TIM2->CR1 = 0x10; TIM2->CR1 |= TIM_CR1_UDIS; - TIM2->RCR |= 0x001; // update event once per up/down count of TIM2 TIM2->EGR |= TIM_EGR_UG; - TIM2->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock - TIM2->ARR = TMR2_COUNT-1; // set auto reload, 5 khz + TIM2->PSC = 2-1; // 20 prescaler, 5kHz + TIM2->ARR = 1; TIM2->CNT = 0; TIM2->SR = 0; + TIM2->SMCR = 0x07; //External clock mode TIM2->CR1 |= TIM_CR1_CEN; // enable TIM2 + + + ////////////////Trigger by Internal Clock///////////////////////////////// +// RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // enable TIM2 clock +// //ISR Setup +// NVIC_EnableIRQ(TIM2_IRQn); //Enable TIM2 IRQ +// TIM2->DIER |= TIM_DIER_UIE; // enable update interrupt +// TIM2->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode +// TIM2->CR1 |= TIM_CR1_UDIS; +// TIM2->RCR |= 0x001; // update event once per up/down count of TIM2 +// TIM2->EGR |= TIM_EGR_UG; +// TIM2->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock +// TIM2->ARR = TMR2_COUNT-1; // set auto reload, 5 khz +// TIM2->CNT = 0; +// TIM2->SR = 0; +// TIM2->CR1 |= TIM_CR1_CEN; // enable TIM2 } void Init_TMR1() { - RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM4 clock + ///////////////////PWM output by L6205D///////////////////////////////// + + RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock FastPWM pwm_v(PIN_H1); FastPWM pwm_w(PIN_L1); - + TIM1->CR2 |= 0x20; //MMS = 010 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode @@ -196,9 +231,9 @@ TIM1->CNT = 0; TIM1->SR = 0; TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1 - + ///////////////////PWM output by IRSM045MA///////////////////////////////// // RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock // // FastPWM pwm_H1(PIN_H1); @@ -245,7 +280,8 @@ TIM4->CR1 |= TIM_CR1_UDIS; TIM4->EGR |= TIM_EGR_UG; - TIM4->PSC = 10-1; // 10 prescaler, timer counts up in sync with the peripheral clock +// TIM4->PSC = 10-1; // 10 prescaler, 1kHz + TIM4->PSC = 20-1; // 20 prescaler, 500Hz TIM4->ARR = 1; TIM4->CNT = 0; TIM4->SR = 0;
--- a/main.cpp Wed Jul 20 07:50:48 2022 +0000 +++ b/main.cpp Tue Jul 26 06:58:25 2022 +0000 @@ -270,7 +270,7 @@ dac_1 = PRES_A_VREF / 3.3f; dac_2 = PRES_B_VREF / 3.3f; } - + make_delay(); for (int i=0; i<50; i++) { @@ -356,9 +356,9 @@ } } Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; - int R_FET = 1.5f; - int R_Motor = 5.4f; - Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; +// int R_FET = 1.5f; +// int R_Motor = 5.4f; +// Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; } // Valve feedforward for SW valve @@ -390,9 +390,9 @@ } } Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; - int R_FET = 1.5f; - int R_Motor = 5.4f; - Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; +// int R_FET = 1.5f; +// int R_Motor = 5.4f; +// Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; } // PWM duty vs. voltage output of L6205 in STM board @@ -541,18 +541,26 @@ LVDT_L = 0; LVDT_H = 1; - for (int ij = 0; ij<150; ij++) { - + for (int ij = 0; ij<120; ij++) { + if (ij < 20) { + continue; + } else if (ij == 20) { +// LED = 1; + } else if (ij == 100) { + LVDT_H = 0; + LVDT_L = 0; + } ADC3->CR2 |= 0x40000000; LVDT_new = ((float)ADC3->DR - 2047.0f); if(DIR_VALVE_ENC < 0) LVDT_new = 0.0f - LVDT_new; LVDT_sum = LVDT_sum + LVDT_new; } +// LED = 0; - LVDT_H = 0; - LVDT_L = 1; +// LVDT_H = 0; +// LVDT_L = 0; - LVDT_new = (LVDT_sum + 2047.0f * 150.0f) * 0.01f*2.0f; + LVDT_new = (LVDT_sum + 2047.0f * 100.0f) * 0.01f; float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f)); LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new; @@ -570,10 +578,11 @@ float FREQ_TMR2 = (float)FREQ_5k; float DT_TMR2 = (float)DT_5k; int cnt_trans = 0; +int cnt_jitter = 0; extern "C" void TIM2_IRQHandler(void) { if (TIM2->SR & TIM_SR_UIF ) { - + LED = 1; if(MODE_POS_FT_TRANS == 1) { if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f; @@ -797,9 +806,9 @@ Vout.ref = 3000.0f * sin(2.0f*3.14159f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { Vout.ref = 1000.0f*(ID_index_array[ID_index]); - int R_FET = 1.5f; - int R_Motor = 5.4f; - Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; +// int R_FET = 1.5f; +// int R_Motor = 5.4f; +// Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; @@ -1394,13 +1403,15 @@ } else { //////////////////////////sw valve // Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW - // L6205PD -// if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f; -// else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f; -// else V_out = 0.0f; - // IRSM836-45MA - if (V_out > 0 ) V_out = V_out*1.654f + 200.0f; - else if (V_out < 0) V_out = V_out*1.654f - 200.0f; + + /////////////Jitter///////////////////// + V_out = V_out + 4000.0f*sin(2.0f*3.14159f*400.0f*((float) cnt_jitter)*0.0002f); //4000, 500Hz + cnt_jitter++; + if(cnt_jitter > 50000000) cnt_jitter = 0; + + //L6205D + if (V_out > 0 ) V_out = 800.0f + V_out*1.1275f; + else if (V_out < 0) V_out = -800.0f + V_out*1.1275f; else V_out = 0.0f; } @@ -1417,6 +1428,7 @@ } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) { V_out = -VALVE_VOLTAGE_LIMIT*1000.0f; } + PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Saturation of output voltage @@ -1470,6 +1482,7 @@ TMR2_COUNT_CAN_TX = 0; } TMR2_COUNT_CAN_TX++; + LED = 0; } TIM2->SR = 0x0; // reset the status register }