mirror of
https://gitee.com/yyz_o/bk_bishe_pi.git
synced 2025-09-07 23:21:26 +00:00
148 lines
2.6 KiB
Plaintext
148 lines
2.6 KiB
Plaintext
#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);
|
|
}
|
|
|
|
|
|
|