Diff: mycontroller.cpp

Revision:

0:63c6db89607f

Child:

2:d0606d66af96

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mycontroller.cpp	Fri Jun 03 13:51:01 2016 +0000
@@ -0,0 +1,100 @@
+#include "mycontroller.h"
+
+//const int pwmSin[] = {128, 132, 136, 140, 143, 147, 151, 155, 159, 162, 166, 170, 174, 178, 181, 185, 189, 192, 196, 200, 203, 207, 211, 214, 218, 221, 225, 228, 232, 235, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 235, 232, 228, 225, 221, 218, 214, 211, 207, 203, 200, 196, 192, 189, 185, 181, 178, 174, 170, 166, 162, 159, 155, 151, 147, 143, 140, 136, 132, 128, 124, 120, 116, 113, 109, 105, 101, 97, 94, 90, 86, 82, 78, 75, 71, 67, 64, 60, 56, 53, 49, 45, 42, 38, 35, 31, 28, 24, 21, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 21, 24, 28, 31, 35, 38, 42, 45, 49, 53, 56, 60, 64, 67, 71, 75, 78, 82, 86, 90, 94, 97, 101, 105, 109, 113, 116, 120, 124};
+
+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);
+
+
+
+brushlessservo::brushlessservo(/*PwmOut phaseA,PwmOut phaseB,PwmOut &phaseC,DigitalOut &EENNAABLE1,DigitalOut &EENNAABLE2*/)
+{
+/*    
+    pwm[1]=phaseA;
+    pwm[2]=phaseB;
+    pwm[3]=phaseC;
+    enableA=EENNAABLE1;
+    enableB=EENNAABLE2;
+*/  
+    
+    sineArraySize = sizeof(pwmSin)/sizeof(int); 
+    int phaseShift = sineArraySize / 3;
+    
+    enableA=1;
+    enableB=1;
+    
+    
+    phase[0]=0;
+    phase[1]=phase[0]+phaseShift;
+    phase[2]=phase[1]+phaseShift;
+    
+    direction=1;
+    
+    for(int i = 0;i<3;i++){
+        pwm[i]=phase[i];
+        pwm[i].period(1/20000.0);
+        };
+        
+    realtheta=0;
+    
+}
+
+void brushlessservo::gotothetha(int32_t theta)
+{
+    int32_t theta_diff;
+    theta_diff = theta - realtheta;
+    
+    if(theta_diff<0)
+    {
+        direction=0;
+        theta_diff=-theta_diff;    
+    }
+    else
+    {
+        direction=1;
+    }
+    
+    
+    for(int i =0;i<theta_diff/5;i++)
+    {
+        for(int j=0;j<3;j++)
+        {
+            if(direction==1){
+                phase[j]+=1;
+                phase[j]%=sineArraySize;
+            } 
+            else
+            {
+                phase[j]-=1;
+                phase[j]%=sineArraySize;
+                if(phase[j]<0)
+                {
+                    phase[j]+=sineArraySize;
+                }
+            }       
+            pwm[j]=pwmSin[phase[j]]/255.0;    
+        }
+        direction?realtheta+=5:realtheta-=5;
+        wait_ms(1);
+    }
+}
+
+
+void brushlessservo::test()
+{
+    for(int j=0;j<3;j++){
+        pwm[j]=0.5;
+    }
+        
+}
+
+
+
+void brushlessservo::updatepwm()
+{
+       
+}
\ No newline at end of file