Merge branch 'master' of https://codehub.devcloud.cn-east-3.huaweicloud.com/2e32552ddb1a4a45b5e8c3c0e5705b48/bk_bishe_pi

文件/老师的函数.txt

@@ -0,0 +1,147 @@
+#include "eyebot.h"
+#define SAFE 200
+
+static int v_des1,v_des2;
+
+float Kp=0.25;
+float Ki=0.06;
+float Klink=0.05;
+
+TIMER t1;
+
+void picontroller()
+{
+    int enc_new1,v_act1,r_mot1,e_func1;
+    int enc_new2,v_act2,r_mot2,e_func2;
+    static int enc_old1,enc_old2;
+    static int r_old1=0,e_old1=0;
+    static int r_old2=0,e_old2=0;
+    
+    enc_new1=ENCODERRead(1);
+    enc_new2=ENCODERRead(2);
+    
+    v_act1=enc_new1-enc_old1;
+    v_act2=enc_new2-enc_old2;
+    
+    e_func1=v_des1-v_act1;
+    e_func2=v_des2-v_act2;
+    
+
+    r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2));
+    r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2));
+    
+    if(v_des1>0&&v_des2>0)
+    {
+        if(r_mot1>35)r_mot1=35;
+        else if(r_mot1<0) r_mot1=0;
+    
+        if(r_mot2>35)r_mot2=35;
+        else if(r_mot2<0) r_mot2=0;
+    }
+    else if(v_des1>0&&v_des2<0)
+    {
+        if(r_mot1>35)r_mot1=35;
+        else if(r_mot1<0) r_mot1=0;
+        if(r_mot2>0)r_mot2=0;
+        else if(r_mot2<-35) r_mot2=-35;
+    }
+    else if(v_des1<0&&v_des2>0)
+    {
+        if(r_mot1>0)r_mot1=0;
+        else if(r_mot1<-35) r_mot1=-35;
+        if(r_mot2>35)r_mot2=35;
+        else if(r_mot2<0) r_mot2=0;
+    }
+    else if(v_des1==0&&v_des2==0)
+    {
+        r_mot1=0;
+        r_mot2=0;
+        v_act1=0;
+        v_act2=0;
+        enc_old1=0;
+        r_old1=0;
+        enc_old2=0;
+        r_old2=0;
+        e_func1=0;
+        e_func2=0;
+        ENCODERReset(1);
+        ENCODERReset(2);
+    }
+    
+    MOTORDrive(1,r_mot1);
+    MOTORDrive(2,r_mot2);    
+    
+    enc_old1=enc_new1;
+    r_old1=r_mot1;
+    e_old1=e_func1;
+    
+    enc_old2=enc_new2;
+    r_old2=r_mot2;
+    e_old2=e_func2;
+    
+    printf("%d    %d    %d     %d\n",r_mot1,r_mot2,enc_old1,enc_old2);
+    
+}
+
+void VWSpeed(int v1,int v2)
+{
+    v_des1=v1;
+    v_des2=v2;
+}
+
+
+void drive()
+{
+   do
+   {
+        if(PSDGetRaw(PSD_LEFT)<SAFE)
+            VWSpeed(100,90);
+        else VWSpeed(90,100);
+        
+        OSWait(80);
+        
+    }while(PSDGetRaw(PSD_FRONT)>SAFE);
+    VWSpeed(0,0);
+    OSWait(1000);
+}
+
+void turn()
+{
+    VWSpeed(50,-50);
+    while(PSDGetRaw(PSD_FRONT)<400) OSWait(80);
+    VWSpeed(0,0);
+    OSWait(1000);
+}
+
+int main() 
+{
+    LCDMenu("STOP","2","3","END");
+    ENCODERReset(1);
+    ENCODERReset(2);
+    
+    PSDGetRaw(PSD_FRONT);
+    PSDGetRaw(PSD_LEFT);
+    
+    
+    t1=OSAttachTimer(100,picontroller);
+
+    while(KEYRead()!=KEY1)
+    { drive();
+      turn();
+    }
+    
+    OSDetachTimer(t1);
+    
+    AUBeep();
+
+    while(KEYRead()!=KEY4)
+    {
+    MOTORDrive(1,0);
+    MOTORDrive(2,0);
+    }
+
+    return(0);
+ }
+ 
+   
+