Diff: main.cpp

Revision:

0:320025ba637f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 24 23:41:45 2017 +0000
@@ -0,0 +1,362 @@
+#include "mbed.h"
+#include <math.h>
+#include "rtos.h"
+#define TS1 0.2
+
+int i=0,q=0,s=0,r=0,rr=0,START=3; //5
+int sensorless=0;
+float wsi=0,wsj=0;
+float t=0,tt=0,tp=0;
+float t1=0,t2=0,t3=0,t4=0,t5=0,t6=0,t7=0,t8=0,t9=0,t10=0;
+float adc_s=0;
+float Speed=0,Speed_h=0;
+float ad_sensorless;
+float power=1.0;
+
+PwmOut mypwmA(PA_8); //PWM_OUT 
+PwmOut mypwmB(PA_9); 
+PwmOut mypwmC(PA_10);
+
+PwmOut  Current_Ref(PB_4);
+DigitalOut Sensorless(PC_4);
+
+DigitalOut EN1(PC_10);
+DigitalOut EN2(PC_11);
+DigitalOut EN3(PC_12);
+DigitalOut OUTC(PC_8);
+
+
+AnalogIn V_adc(PC_2);  //Outside Volume
+AnalogIn V_sinkaku(PB_1);   //Blue Volume
+AnalogOut SWAVE(PA_4);
+
+
+AnalogIn BEMF1(PC_3);//C7_37
+AnalogIn BEMF2(PB_0);//C7_34
+AnalogIn BEMF3(PA_7);//C10_26 PA_7
+
+DigitalOut GPIO_BEMF(PC_9);
+
+InterruptIn  HA(PA_15);
+
+Timer timer2; 
+Timer timer_uT;
+float ut1=0,ut2=0,usi=0;
+
+Serial pc(USBTX,USBRX);
+
+DigitalOut myled(LED1);
+
+float Vr_adc=0.0f;
+float adc;
+float sinkaku;
+
+void HAH(){
+   
+    rr=r%2;
+    if(rr==0){
+    ut1=timer_uT.read_us();
+    r++; 
+    }
+   
+    if(rr==1){
+    ut2=timer_uT.read_us();
+    r++;
+    timer_uT.reset(); 
+    }  
+      }
+      
+  void CPLT(){     
+  pc.printf("%.3f , %.3f \r" ,Speed_h ,Vr_adc);
+  }
+     
+   void timerTS1(void const*argument){
+   CPLT();
+  }      
+ 
+int main() {
+  
+   EN1=1;
+   EN2=1;
+   EN3=1;
+   
+    timer2.start();
+    timer_uT.start();
+    mypwmA.period_us(20);
+    mypwmB.period_us(20); 
+    mypwmC.period_us(20);
+    
+   OUTC=0;
+   GPIO_BEMF=0;
+   pc.baud(128000);
+   
+  /*   RtosTimer RtosTimerTS1(timerTS1);
+     RtosTimerTS1.start((unsigned int)(TS1*5000)); 
+     Thread::wait(100); */
+ 
+ while(1) {
+      
+      HA.rise(&HAH);
+      Vr_adc=V_adc.read();
+      adc= V_adc.read_u16()>>4 ; 
+      
+      sinkaku=((V_sinkaku.read_u16())>>7)+20;//7
+      
+  if(i==0){      
+   mypwmA.write(0.5f);
+   wait_ms(100);    
+   i=1;
+   }
+   
+    tp=3800-(adc);//  3800 3300-4000
+    
+   if(sensorless==0){
+     t=tp;
+     tt=t;
+    }  
+          
+   
+    
+    if(Speed<900){ //900
+      // t=tp;
+       sensorless=0;
+       s=1;
+       }
+    if(Speed>=900){ //900
+          
+          if(s==1){
+          ad_sensorless=Vr_adc;
+          s=0;      
+          sensorless=1;
+          t1=(abs(wsj-wsi));
+          
+          t=t1; 
+         // tt=t;       
+           }
+           
+       if((Speed >= 900)&&(Speed<=1500)){     
+         power=0.8;  //0.5
+         }
+         if((Speed >= 1500)&&(Speed<=2000)){     
+         power=0.6;  //0.5
+         }
+        if((Speed >= 2000)&&(Speed<=2500)){     
+         power=0.8;  //0.6
+         } 
+         if((Speed >= 2500)){     
+         power=1.0;  //0.7
+         
+         } 
+       
+         
+        adc_s=Vr_adc-ad_sensorless; 
+         
+        if(adc_s < -0.01){
+          t2=t1+100;
+          t=t2;
+          tt=t-sinkaku;
+           }  
+       
+       if((adc_s > 0.0)&&(adc_s <= 0.05)){
+          t2=t1-200; //200
+          t=t2;
+           tt=t-sinkaku;
+           } 
+       if((adc_s > 0.05)&&(adc_s <= 0.1)){
+          t3=t2-150; //150
+          t=t3;
+           tt=t-sinkaku;
+           } 
+        if((adc_s > 0.1)&&(adc_s <= 0.15)){
+          t4=t3-130; //130
+          t=t4;
+           tt=t-sinkaku;
+           } 
+         if((adc_s > 0.15)&&(adc_s <= 0.2)){
+          t5=t4-100; //120
+          t=t5;
+           tt=t-sinkaku;
+           }
+          if((adc_s > 0.2)&&(adc_s <= 0.25)){
+          t6=t5-100; //100
+          t=t6;
+           tt=t-sinkaku;
+          }
+           if((adc_s > 0.25)&&(adc_s <= 0.3)){
+          t7=t6-80; //80
+          t=t7;
+           tt=t-sinkaku;
+           }
+          if((adc_s > 0.3)&&(adc_s <= 0.35)){              
+          t8=t7-70; //70
+          t=t8;
+           tt=t-sinkaku;
+           }
+          if((adc_s > 0.35)&&(adc_s <= 0.4)){ 
+          t9=t8-70; //70
+          t=t9;
+           tt=t-sinkaku;
+           } 
+          if((adc_s > 0.4)&&(adc_s <= 0.45)){ 
+          t10=t9-50; //50
+          t=t10;
+           tt=t-sinkaku;
+           } 
+          if(adc_s > 0.45){ 
+          t=t10-50; //50
+           tt=t-sinkaku;
+           } 
+       }
+      
+         // tt=t-100;
+       
+ 
+      
+ if((Vr_adc>0.15f)&&(q==0)){     
+  while(q<100){   
+   
+    EN1=1; 
+    EN2=1;
+    EN3=0;
+    mypwmA.write(0.5f);//0.5
+    mypwmB=0;
+    mypwmC=0;
+   
+    wait_ms(START);
+    
+    EN1=1;  
+    EN2=0;  
+    EN3=1;
+    
+    wait_ms(START);
+    
+    EN1=0; 
+    EN2=1;
+    EN3=1;
+    mypwmA=0;
+    mypwmB.write(0.5f);
+    mypwmC=0;
+    wait_ms(START);
+   
+    EN1=1; 
+    EN2=1;
+    EN3=0;
+    
+    wait_ms(START);
+    
+    EN1=1; 
+    EN2=0;
+    EN3=1;
+    
+    mypwmA=0;
+    mypwmB=0;
+    mypwmC.write(0.5f);
+    
+    wait_ms(START);
+   
+    EN1=0; 
+    EN2=1;
+    EN3=1;
+   
+    q++;
+    wait_ms(START);
+    
+    } 
+    }
+      
+ if(Vr_adc < 0.05f){ //0.005
+          q=0;      
+          s=0;
+          r=0;
+          rr=0;
+          i=0;  
+          sensorless=0;
+          power=1.0;
+       }  
+       
+
+       
+ if(Vr_adc >0.05f){
+      EN1=1; 
+      EN2=1; 
+      EN3=0;    
+       
+      mypwmA.write(Vr_adc*power); 
+      mypwmB.write(0.0f);
+      mypwmC.write(0.0f);
+     
+      wait_us(t-200); 
+       EN1=1; 
+       EN2=0; 
+       EN3=1; 
+   
+    if(BEMF1>0.5f){ 
+          OUTC=0;        
+      wsi=timer2.read_us(); 
+           }
+      wait_us(tt);
+      if(OUTC==0){
+      wsj=timer2.read_us(); 
+      }
+      OUTC=1;
+   
+      EN1=0; 
+      EN2=1; 
+      EN3=1; 
+     
+       mypwmA.write(0.0f);
+       mypwmB.write(Vr_adc*power); 
+       mypwmC.write(0.0f);
+      
+      wait_us(t);
+   
+      EN1=1;
+      EN2=1;
+      EN3=0;
+     
+    if(BEMF2>0.5f){ 
+          OUTC=0;        
+      wsi=timer2.read_us(); 
+           }
+      wait_us(tt);
+      if(OUTC==0){
+      wsj=timer2.read_us(); 
+      }
+      OUTC=1;
+   
+       EN1=1; 
+       EN2=0;
+       EN3=1; 
+    
+       mypwmA.write(0.0f);
+       mypwmB.write(0.0f);
+       mypwmC.write(Vr_adc*power); 
+    
+        wait_us(t);
+   
+       EN1=0; 
+       EN2=1; 
+       EN3=1; 
+      
+     
+    if(BEMF3>0.5f){ 
+          OUTC=0;        
+      wsi=timer2.read_us(); 
+           }
+      wait_us(tt);
+      if(OUTC==0){
+      wsj=timer2.read_us(); 
+      }
+      OUTC=1;   
+         
+   }
+   
+         Sensorless=sensorless;
+         usi=abs(ut2-ut1);
+         Speed_h=60*(1/(7.0*usi*1E-6));
+         Speed=60*(1/(7.0*6.0*t*1E-6));
+        
+        myled = !myled;
+       
+    }
+}