Revision 2:146ff0747375, committed 2016-06-03

Comitter:

ThomasGS

Date:

Fri Jun 03 14:50:37 2016 +0000

Parent:

1:f5b18e784a39

Commit message:

version avec moteur parametre

Changed in this revision

--- a/main.cpp	Fri Jun 03 13:51:54 2016 +0000
+++ b/main.cpp	Fri Jun 03 14:50:37 2016 +0000
@@ -7,13 +7,22 @@
 Ticker filter; 
 Ticker toggler1; 
 MPU6050 mpu6050;  
-/*
+
 PwmOut pwm1A(PA_10);
 PwmOut pwm1B(PA_6);
 PwmOut pwm1C(PB_6);
 DigitalOut enable1A(PA_7);
 DigitalOut enable1B(PA_9);
-*/
+
+PwmOut pwm2A(PB_1);
+PwmOut pwm2B(PB_10);
+PwmOut pwm2C(PB_5);
+DigitalOut enable2A(PB_4);
+DigitalOut enable2B(PC_4);
+
+brushlessservo moteur(pwm1A,pwm1B,pwm1C,enable1A,enable1B);
+brushlessservo moteurB(pwm2A,pwm2B,pwm2C,enable2A,enable2B);
+
 float pitchAngle = 0;
 float rollAngle = 0;
 
@@ -22,7 +31,7 @@
 void toggle_led2();
 
 int main() {
-    brushlessservo moteur;
+    //brushlessservo moteur;
     mpu6050.init(); 
     filter.attach(&compFilter, 0.005);
     wait(1);

--- a/mycontroller.cpp	Fri Jun 03 13:51:54 2016 +0000
+++ b/mycontroller.cpp	Fri Jun 03 14:50:37 2016 +0000
@@ -5,22 +5,20 @@
 const int pwmSin[102] = {127,135,143,150,158,166,173,180,187,194,201,207,213,219,224,229,233,237,241,244,247,250,252,253,254,254,254,254,253,252,250,247,244,241,237,233,229,224,219,213,207,201,194,187,180,173,166,158,150,143,135,127,119,111,104,96,88,81,74,67,60,53,47,41,35,30,25,21,17,13,10,7,4,2,1,0,0,0,0,1,2,4,7,10,13,17,21,25,30,35,41,47,53,60,67,74,81,88,96,104,111,119};
 
 
-PwmOut pwm[] = {(PA_10),(PA_6),(PB_6)};
-DigitalOut enableA(PA_7);
-DigitalOut enableB(PA_9);
+//PwmOut pwm[] = {(PA_10),(PA_6),(PB_6)};
+//DigitalOut enableA(PA_7);
+//DigitalOut enableB(PA_9);
 
+PwmOut pwm[] = {(PB_1),(PB_10),(PB_5)};
+DigitalOut enableA(PB_4);
+DigitalOut enableB(PC_4);
 
 
-brushlessservo::brushlessservo(/*PwmOut phaseA,PwmOut phaseB,PwmOut &phaseC,DigitalOut &EENNAABLE1,DigitalOut &EENNAABLE2*/)
+brushlessservo::brushlessservo(PwmOut &p_pwm1,PwmOut &p_pwm2,PwmOut &p_pwm3,DigitalOut &p_enableA,DigitalOut &p_enableB) : pwm1(p_pwm1), pwm2(p_pwm2), pwm3(p_pwm3), enableA(p_enableA), enableB(p_enableB)
 {
-/*    
-    pwm[1]=phaseA;
-    pwm[2]=phaseB;
-    pwm[3]=phaseC;
-    enableA=EENNAABLE1;
-    enableB=EENNAABLE2;
-*/  
-    
+//brushlessservo::brushlessservo()
+//{
+        
     sineArraySize = sizeof(pwmSin)/sizeof(int); 
     int phaseShift = sineArraySize / 3;
     
@@ -33,12 +31,20 @@
     phase[2]=phase[1]+phaseShift;
     
     direction=1;
-    
+    /*
     for(int i = 0;i<3;i++){
-        pwm[i]=phase[i];
+        pwm[i]=pwmSin[phase[i]]/255.0;
         pwm[i].period(1/20000.0);
         };
-        
+    */
+    pwm1.period(1/20000.0);
+    pwm2.period(1/20000.0);
+    pwm3.period(1/20000.0);
+    
+    pwm1=pwmSin[phase[0]]/255.0;
+    pwm2=pwmSin[phase[1]]/255.0;
+    pwm3=pwmSin[phase[2]]/255.0;
+    
     realtheta=0;
     
 }
@@ -61,7 +67,7 @@
     
     for(int i =0;i<theta_diff/5;i++)
     {
-        for(int j=0;j<3;j++)
+     /*   for(int j=0;j<3;j++)
         {
             if(direction==1){
                 phase[j]+=1;
@@ -78,6 +84,40 @@
             }       
             pwm[j]=pwmSin[phase[j]]/255.0;    
         }
+        */
+        if(direction==1){
+                phase[0]+=1;
+                phase[1]+=1;
+                phase[2]+=1;
+                
+                phase[0]%=sineArraySize;
+                phase[1]%=sineArraySize;
+                phase[2]%=sineArraySize;
+            } 
+            else
+            {
+                phase[0]-=1;
+                phase[1]-=1;
+                phase[2]-=1;
+                
+                phase[0]%=sineArraySize;
+                phase[1]%=sineArraySize;
+                phase[2]%=sineArraySize;
+                if(phase[0]<0){
+                    phase[0]+=sineArraySize;
+                }
+                if(phase[1]<0){
+                    phase[1]+=sineArraySize;
+                }
+                if(phase[2]<0){
+                    phase[2]+=sineArraySize;
+                }
+            }       
+            pwm1=pwmSin[phase[0]]/255.0;   
+            pwm2=pwmSin[phase[1]]/255.0;   
+            pwm3=pwmSin[phase[2]]/255.0;   
+        
+        
         direction?realtheta+=5:realtheta-=5;
         wait_ms(1);
     }
@@ -86,10 +126,11 @@
 
 void brushlessservo::test()
 {
+    /*
     for(int j=0;j<3;j++){
         pwm[j]=0.5;
     }
-        
+      */  
 }
 
 

--- a/mycontroller.h	Fri Jun 03 13:51:54 2016 +0000
+++ b/mycontroller.h	Fri Jun 03 14:50:37 2016 +0000
@@ -6,12 +6,16 @@
 class brushlessservo
 {
     public:
-        brushlessservo();
+        brushlessservo(PwmOut &p_pwm1,PwmOut &p_pwm2,PwmOut &p_pwm3,DigitalOut &p_enableA,DigitalOut &p_enableB);
+        //brushlessservo();
         void gotothetha(int theta);
         void updatepwm();
         void test();
         
     private:
+        DigitalOut &enableA,&enableB;
+        PwmOut pwm[3];
+        PwmOut &pwm1, &pwm2, &pwm3;
         int32_t realtheta;
         int32_t targettheta;
         int32_t phase[3];