Diff: main.cpp

Revision:

2:a1c0a37df760

Parent:

1:e04e563be5ce

Child:

5:a4319f79457b

diff -r e04e563be5ce -r a1c0a37df760 main.cpp
--- a/main.cpp	Mon Aug 19 08:05:09 2019 +0000
+++ b/main.cpp	Tue Aug 20 07:38:55 2019 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "FastPWM.h"
 #include "INIT_HW.h"
+#include "function_CAN.h"
 #include "SPI_EEP_ENC.h"
 #include "I2C_AS5510.h"
 #include "setting.h"
@@ -10,10 +11,11 @@
 DigitalOut check_2(PC_3);
 AnalogOut dac_1(PA_4);
 AnalogOut dac_2(PA_5);
+AnalogIn adc3(PC_1);
 
 // pwm
-float dtc_v=0;
-float dtc_w=0;
+double dtc_v=0.0;
+double dtc_w=0.0;
 
 // I2C
 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
@@ -25,6 +27,7 @@
 DigitalOut eeprom_cs(PB_12);
 SPI enc(PC_12,PC_11,PC_10);
 DigitalOut enc_cs(PD_2);
+DigitalOut indi_led(PA_15);
 
 // UART
 Serial pc(PA_9,PA_10); //  _ UART
@@ -32,52 +35,57 @@
 //CAN
 CAN can(PB_8, PB_9, 1000000);
 CANMessage msg;
-void onMsgReceived()
-{
-    can.read(msg);
-    pc.printf("Message received: %d\n", msg.data[0]);
-}
+
+// Variables
 
 
-//Timer t;
-//t.start();
-//t.stop();
-//pc.printf("The time taken was %f seconds\n",t.read());
-int a1;
-float PWM_out;
-
+double a1;
+double PWM_out;
+double temp_cur=0.0;
 int main()
 {
     /*******************************************************
     ***     Initialization
     ********************************************************/
         
+    indi_led = 0;
     pc.baud(9600);
+    
+    //Timer t;
+    //t.start();
+    //t.stop();
+    //pc.printf("The time taken was %f seconds\n",t.read());
+
     // i2c init
 //    i2c.frequency(400 * 1000);          // 0.4 mHz
 //    wait_ms(2);                         // Power Up wait
 //    look_for_hardware_i2c();            // Hardware present
 //    init_as5510(i2c_slave_addr1);
+
 //    // spi init
     eeprom.format(8,3);
     eeprom.frequency(5000000); //5M
     enc.format(8,0);
     enc.frequency(5000000); //5M
+    
     // ADC init
-    Init_ADC();
+//    Init_ADC();
+
     // Pwm init
     Init_PWM();
     TIM4->CR1 ^= TIM_CR1_UDIS;
+    
 //    //SPI
 //    spi_eeprom_ready();
 //    spi_eeprom_write(0x1,0x112);
 //    spi_eeprom_ready();
 //    int i = spi_eeprom_read(0x1);
+
     // CAN
-    can.attach(&onMsgReceived);
+    can.attach(&CAN_RX_HANDLER);
+    
     // spi _ enc
     spi_enc_set_init();
-    msg.len=2;
     
     
     /*******************************************************
@@ -95,38 +103,52 @@
         //spi _ enc
         int a = spi_enc_read();
 //        read_field(i2c_slave_addr1);
-        pc.printf("7 \n");
-//        msg.data[0]=0xFF&a1;
-//        msg.data[1]=0xFF&(a1>>8);
-//        can.write(msg);
+
         check_2=0;
-//        wait(0.00005f);
+
+//        pc.printf("%f\n",PWM_out);
+//        pc.printf("%d\n",a1);
+        
+//        wait(0.01f);
     }
 }
 
-unsigned int CNT_TMR4 = 0;
+/*******************************************************************************
+                            TIMER INTERRUPT
+*******************************************************************************/
+
+unsigned long CNT_TMR4 = 0;
+double FREQ_TMR4 = (double)FREQ_20k;
 extern "C" void TIM4_IRQHandler(void)
 {
-    if (TIM4->SR & TIM_SR_UIF ) {
+    if ( TIM4->SR & TIM_SR_UIF ) {
         /*******************************************************
         ***     Sensor Read & Data Handling 
         ********************************************************/
         
-        //spi
-//      eeprom.write(0xff);
-//      eeprom.write(0xff);
-//      ready();
-//      read(1);
+        if((CNT_TMR4%2)==0){
+            //spi
+    //      eeprom.write(0xff);
+    //      eeprom.write(0xff);
+    //      ready();
+    //      read(1);
+    
+            //i2c
+    ////    read_field(i2c_slave_addr1);
+    
+            //ADC
+    //        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
+    //      a1=ADC1->DR;
+    //      a1=ADC2->DR;
+    //        int raw_cur = ADC3->DR;
+            double raw_cur=(double)adc3*4096.0;
+        
+            double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*100.0)); // f_cutoff : 500Hz
+            double new_a1 = (double)raw_cur;
+            a1=a1*(1.0-alpha_update_cur)+new_a1*(alpha_update_cur);
+//            a1=new_a1;
+        }
 
-        //i2c
-////    read_field(i2c_slave_addr1);
-
-        //ADC
-        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
-//      a1=ADC1->DR;
-//      a1=ADC2->DR;
-        a1=ADC3->DR;
-        
         //DAC
 //      dac_1 = ADC1->DR;
 //      dac_2 = ADC2->DR;
@@ -135,34 +157,54 @@
         ***     Valve Control 
         ********************************************************/
         
-//        PWM_out=-0.04683;
-        PWM_out=-0.95;
-
-        if (PWM_out>0) {
-            dtc_v=0;
+        // PWM_out : -1.0~1.0(full duty)
+        PWM_out = -0.0104;
+        double t = (double)CNT_TMR4/FREQ_TMR4;
+        double T = 10.0; 
+//        PWM_out = 0.36*sin(2.0*PI*t/T);
+        PWM_out = 0.04*sin(2.0*PI*t/T);
+//        if(PWM_out > 0.0) PWM_out = 0.02;
+//        else if(PWM_out < 0.0) PWM_out = -0.02; 
+        
+        // Saturation of output voltage to 5.0V
+        if(PWM_out > (5.0/12.0)) PWM_out=(5.0/12.0);
+        else if (PWM_out < (-5.0/12.0)) PWM_out=(-5.0/12.0);
+        
+        if (PWM_out>0.0) {
+            dtc_v=0.0;
             dtc_w=PWM_out;
         } else {
-            dtc_v=0;
-            dtc_w=-PWM_out;
+            dtc_v=-PWM_out;
+            dtc_w=0.0;
         }
         
         //pwm
-        TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
-        TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
+        TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
+        TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
         
         /*******************************************************
-        ***     Data Send (CAN)
+        ***     Data Send (CAN) & Print out (UART)
         ********************************************************/
+        if((CNT_TMR4%40)==0){
+            msg.id = 50;
+            msg.len = 4;
+            int temp_CUR = (int)(a1);
+            msg.data[0]=0x00FF&temp_CUR;
+            msg.data[1]=0x00FF&(temp_CUR>>8);
+            int temp_PWM = (int)(PWM_out*10000.0);
+            msg.data[2]=0x00FF&temp_PWM;
+            msg.data[3]=0x00FF&(temp_PWM>>8);
+            can.write(msg);
+        }
 
-
-
+//        if((CNT_TMR4%4000)==0){
+//            pc.printf("%d\n",a1);
+//        }
 
         /*******************************************************
         ***     Timer Counting & etc.
         ********************************************************/
-        if(CNT_TMR4 == 100000) CNT_TMR4 = 0;
-        else CNT_TMR4++; 
-
+        CNT_TMR4++; 
     }
     TIM4->SR = 0x0;  // reset the status register
 }
\ No newline at end of file