rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

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
 }