Diff: main.cpp

Revision:

0:d7338aa3fbcd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 13 06:00:01 2020 +0000
@@ -0,0 +1,238 @@
+#include "mbed.h"
+#include <math.h>
+#include "rtos.h"
+
+#define PI 3.14159265358979
+#define TS1 0.01//0.2
+
+int i=0,q=0;
+
+short f1=0,a1=0;
+int START=10;//8
+float ut1=0,ut2=0,usi=0;
+float vt1=0,vt2=0,vsi=0;
+float wt1=0,wt2=0,wsi=0;
+
+float Speed;
+float tau=100;  //60
+float su,sv,sw;
+
+PwmOut mypwmA(PA_8); //PWM_OUT 8
+PwmOut mypwmB(PA_9); //9
+PwmOut mypwmC(PA_10);//10
+
+DigitalOut EN1(PC_10);
+DigitalOut EN2(PC_11);
+DigitalOut EN3(PC_12);
+
+InterruptIn  HA(PA_15);
+InterruptIn  HB(PB_3);
+InterruptIn  HC(PB_10);
+
+AnalogOut aout(PA_4);
+
+//AnalogIn V_adc(PB_1);//Volume
+AnalogIn V_adc(PC_2); // gaibu Volume
+
+AnalogIn Current_W(PC_0);
+
+float Vr_adc,output,Current_Ws;
+float zint=tau*1E-6; //40E-6 MaX
+Serial pc(USBTX,USBRX);
+
+DigitalOut myled(LED1);
+
+Timer uT;
+Timer vT;
+Timer wT;
+   
+Ticker zt;
+
+     short u[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 
+     short v[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 
+     short w[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
+
+void ztrans(){
+    
+            u[2] = ((u[1]*u[4])>>14) + ((u[0]*u[3])>>14);
+            u[4] = u[3]; 
+            u[3] = u[2];          
+       
+            v[2] = ((v[1]*v[4])>>14) + ((v[0]*v[3])>>14);
+            v[4] = v[3]; 
+            v[3] = v[2];
+            
+            w[2] = ((w[1]*w[4])>>14) + ((w[0]*w[3])>>14);
+            w[4] = w[3]; 
+            w[3] = w[2];
+    
+    }
+    
+void HAH(){
+    ut1=uT.read_us();
+   
+     f1=(sin(2*3.14159*(1/(usi*1E-6))*zint)*16384); //125E-6
+     a1=(2*cos(2*3.14159*(1/(usi*1E-6))*zint)*16384);        
+  
+      u[0]=a1;u[1]=0xC000;u[2]=0; u[3]=f1; u[4]=0;
+      }
+ void HAL(){
+    ut2=uT.read_us();
+    uT.reset();
+     }
+ void HBH(){
+   
+    vt1=vT.read_us();
+   
+    f1=(sin(2*3.14159*(1/(vsi*1E-6))*zint)*16384);
+    a1=(2*cos(2*3.14159*(1/(vsi*1E-6))*zint)*16384);
+         
+      v[0]=a1;v[1]=0xC000;v[2]=0; v[3]=f1; v[4]=0;
+    
+     }
+ void HBL(){
+    vt2=vT.read_us();
+    vT.reset();
+     }
+ void HCH(){
+    wt1=wT.read_us();
+  
+    f1=(sin(2*3.14159*(1/(wsi*1E-6))*zint)*16384);
+    a1=(2*cos(2*3.14159*(1/(wsi*1E-6))*zint)*16384);
+         
+      w[0]=a1;w[1]=0xC000;w[2]=0; w[3]=f1; w[4]=0;
+     }
+     
+ void HCL(){
+    wt2=wT.read_us();
+    wT.reset();
+     } 
+     
+ void CPLT(){
+  //pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
+    // pc.printf("%.3f , %.3f, %.3f\r" ,su,sv,sw);
+ }
+     
+ void timerTS1(void const*argument){
+   CPLT();
+ }    
+
+      
+      
+int main(){  
+
+   pc.baud(128000); 
+    
+   zt.attach_us(&ztrans, tau); //  40MAX
+  
+   EN1=1;
+   EN2=1;
+   EN3=1;
+   
+    mypwmA.period_us(20);
+    mypwmB.period_us(20);
+    mypwmC.period_us(20);
+    
+     uT.start();
+     vT.start();
+     wT.start();   
+     
+     RtosTimer RtosTimerTS1(timerTS1);
+     RtosTimerTS1.start((unsigned int)(TS1*300));//5000
+     Thread::wait(1);//100
+    
+  
+while(1){
+  
+  
+   Vr_adc=V_adc.read();   
+    
+  if((Vr_adc>0.15f)&&(q==0)){     
+  while(q<50){   
+    
+    mypwmA.write(0); //0.8f
+    mypwmB.write(0);
+    mypwmC.write(0.7f);
+    wait_ms(START);
+           
+    mypwmA.write(0);
+    mypwmB.write(0.7f);
+    mypwmC.write(0);
+    wait_ms(START);
+         
+    mypwmA.write(0.7f);
+    mypwmB.write(0);
+    mypwmC.write(0);//0.8f
+    wait_ms(START);
+    q++;
+      }
+      
+     }
+        HA.rise(&HAH);
+        HC.fall(&HCL);
+        HB.rise(&HBH);
+        HA.fall(&HAL);
+        HC.rise(&HCH);
+        HB.fall(&HBL);
+
+    if(Vr_adc < 0.08f){
+          q=0;         
+       }  
+        
+         usi=2*(ut2-ut1); 
+         vsi=2*(vt2-vt1); 
+         wsi=2*(wt2-wt1); 
+      
+      
+         if((u[2])<=-16383){   //飽和処理
+              u[2]=-16383;
+            }
+            if(u[2]>=16383){
+                u[2]=16383;
+                }
+                
+           if(v[2]<=-16383){   //飽和処理
+              v[2]=-16383;
+            }
+           if(v[2]>=16383){
+                v[2]=16383;
+                }
+           if(w[2]<=-16383){  //飽和処理
+               w[2]=-16383;
+            }   
+           if(w[2]>=16383){
+                w[2]=16383;
+                }
+            
+        #if 1 
+        aout=(float(v[2])/(16383*2))*(Vr_adc)+0.5;
+            
+         mypwmA.write(((float(u[2])/(16383*2)))*(Vr_adc)+0.5); 
+         mypwmB.write(((float(v[2])/(16383*2)))*(Vr_adc)+0.5);
+         mypwmC.write(((float(w[2])/(16383*2)))*(Vr_adc)+0.5); 
+         #endif
+         
+        #if 0
+        su=(float(u[2])/(16383*2))*(Vr_adc);
+        sv=(float(v[2])/(16383*2))*(Vr_adc);
+        sw=(float(w[2])/(16383*2))*(Vr_adc);
+      
+          mypwmA.write(su); 
+          mypwmB.write(sv);
+          mypwmC.write(sw); 
+        
+          aout=su;
+          //aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383
+        #endif 
+           
+         Speed=60*(1/(7.0*usi*1E-6));
+               
+            myled = !myled;
+          
+        }  
+  
+         
+}
+
+
+