|
#include "asserv.h"
|
|
|
|
#include <dspic.h>
|
|
#include <math.h>
|
|
#define M_PI 3.14159265358979323846
|
|
|
|
#include "setup.h"
|
|
|
|
float xf,yf;
|
|
float xr,yr;
|
|
|
|
float vitesse;
|
|
float last_t=0, td=0;
|
|
|
|
short ancv1=0,ancv2=0;
|
|
short ancc1=0,ancc2=0;
|
|
short c1,c2;
|
|
short v1,v2;
|
|
|
|
short top_consigne=0, top_vitesse=0;
|
|
|
|
float errp1,errd1,erri1,errt1;
|
|
float errp2,errd2,erri2,errt2;
|
|
short err1,err2;
|
|
|
|
float consV1,consV2;
|
|
float consds,consdt;
|
|
float distance, angle;
|
|
|
|
float dt,ds,tand;
|
|
|
|
unsigned char time_flag=0;
|
|
|
|
Mode mode=Stop;
|
|
Way way=Forward;
|
|
State state=Stopped;
|
|
|
|
unsigned short id=0, cnt=10;
|
|
short tab0[600];
|
|
far short tab1[600], tab2[600];
|
|
far short ydata tab3[600], tab4[600];
|
|
|
|
extern unsigned char arret;
|
|
|
|
#define ANGLE_MAX M_PI/10
|
|
|
|
#ifdef BIG_BOT
|
|
#define ENTRAX 216.7 //TODO 218.5
|
|
//#define TICK1 0.0219196067692
|
|
//#define HTICK 0.0109598033846
|
|
#define TICK1 0.0221513716232
|
|
#define HTICK 0.0110756858116
|
|
#define DELTAVITESSE 0.16
|
|
#define ERR_MINI 100
|
|
#define PWM_MINI 300
|
|
float vitesseMax=32;
|
|
float x =280,y =-250,t=0;
|
|
short kd1=20,ki1=1,kp1=55;
|
|
short kd2=20,ki2=1,kp2=55;
|
|
#elif SMALL_BOT
|
|
#define ENTRAX 103.0
|
|
#define TICK1 0.0219196067692
|
|
#define HTICK 0.0109598033846
|
|
//#define TICK1 0.0221513716232
|
|
//#define HTICK 0.0110756858116
|
|
#define DELTAVITESSE 0.16
|
|
#define ERR_MINI 100
|
|
#define PWM_MINI 200
|
|
float vitesseMax=32;
|
|
float x =280,y =-250,t=0;
|
|
short kd1=0,ki1=1,kp1=40;
|
|
short kd2=0,ki2=1,kp2=40;
|
|
#elif ASSERV_DEG
|
|
#define ENTRAX 219.5
|
|
#define TICK1 0.0223993542529
|
|
#define HTICK 0.0111996771264
|
|
#define DELTAVITESSE 0.16
|
|
#define ERR_MINI 100
|
|
#define PWM_MINI 300
|
|
float vitesseMax=32;
|
|
float x =280,y =-250,t=0;
|
|
short kd1=0,ki1=1,kp1=70;
|
|
short kd2=0,ki2=1,kp2=70;
|
|
#else
|
|
#error "no bot define"
|
|
#endif
|
|
|
|
|
|
void setup_asserv()
|
|
{
|
|
setup_QEI1();
|
|
setup_QEI2();
|
|
setup_PWM1();
|
|
setup_timer1();
|
|
xf=x;
|
|
yf=y;
|
|
}
|
|
|
|
void setInitPos(unsigned short mx, unsigned short my)
|
|
{
|
|
x=mx/10.0;
|
|
y=my/10.0;
|
|
y=-y;
|
|
xf=x;
|
|
yf=y;
|
|
}
|
|
|
|
void setInitTheta(unsigned short mt)
|
|
{
|
|
t=mt/1800.0*M_PI;
|
|
last_t=t;
|
|
}
|
|
|
|
void setDest(unsigned short mx, unsigned short my)
|
|
{
|
|
xf=mx/10.0;
|
|
yf=my/10.0;
|
|
yf=-yf;
|
|
}
|
|
|
|
void setThetaDest(unsigned short mt)
|
|
{
|
|
td=mt/1800.0*M_PI;
|
|
}
|
|
|
|
void setWay(Way mway)
|
|
{
|
|
way=mway;
|
|
}
|
|
|
|
void setMode(Mode mmode)
|
|
{
|
|
mode=mmode;
|
|
}
|
|
|
|
void setVitesse(unsigned char mvitesse)
|
|
{
|
|
vitesse = mvitesse;
|
|
}
|
|
|
|
unsigned short getX()
|
|
{
|
|
return (unsigned short)(x*10.0);
|
|
}
|
|
|
|
unsigned short getY()
|
|
{
|
|
return (unsigned short)(y*-10.0);
|
|
}
|
|
|
|
State getState()
|
|
{
|
|
return state;
|
|
}
|
|
|
|
short getTheta()
|
|
{
|
|
return (short)(t*1800.0/M_PI);
|
|
}
|
|
|
|
unsigned short getDistance()
|
|
{
|
|
return (unsigned short)(distance*10.0);
|
|
}
|
|
|
|
void setTopDest(unsigned short top, unsigned short top_power)
|
|
{
|
|
top_consigne=(short)top;
|
|
top_vitesse=(short)top_power;
|
|
}
|
|
|
|
void setPower(short power)
|
|
{
|
|
if(power>=0)
|
|
{
|
|
m1=power;
|
|
BM1 = 0;
|
|
AM1 = 1;
|
|
}
|
|
else
|
|
{
|
|
m1=-power;
|
|
BM1 = 1;
|
|
AM1 = 0;
|
|
}
|
|
}
|
|
|
|
unsigned short getTop()
|
|
{
|
|
return c2;
|
|
}
|
|
|
|
void asserv(void)
|
|
{
|
|
while(time_flag==0);
|
|
|
|
// rampe vitesse
|
|
//if(vitesse<vitesseMax) vitesse+=DELTAVITESSE;
|
|
|
|
distance = sqrt((x-xf)*(x-xf)+(y-yf)*(y-yf));
|
|
|
|
// FSM
|
|
switch(mode)
|
|
{
|
|
case Stop: // =================== Stop
|
|
consds=0.0;
|
|
consdt=0.0;
|
|
break;
|
|
case Fixe: // =================== Fixe
|
|
// consigne distance
|
|
consds = distance / 4;
|
|
if(consds > vitesse) consds = vitesse;
|
|
|
|
// consigne angle
|
|
if(y-yf<=0)
|
|
{
|
|
if(x-xf<=0) consdt = asin((yf-y)/distance);
|
|
else consdt = -M_PI - asin((yf-y)/distance);
|
|
}
|
|
else
|
|
{
|
|
if(x-xf<=0) consdt = asin((yf-y)/distance);
|
|
else consdt = M_PI - asin((yf-y)/distance);
|
|
}
|
|
consdt = -consdt-t;
|
|
if(consdt>=M_PI/2 || consdt<=-M_PI/2)
|
|
{
|
|
//consdt=M_PI-consdt;
|
|
consds=-consds;
|
|
if(consdt<=3*M_PI/8 & consdt>=-3*M_PI/8)
|
|
{
|
|
consds=0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(consdt>=M_PI/8 || consdt<=-M_PI/8)
|
|
{
|
|
consds=0;
|
|
}
|
|
}
|
|
consdt=last_t-t;
|
|
if(consdt>M_PI) consdt-=2*M_PI;
|
|
if(consdt<-M_PI) consdt+=2*M_PI;
|
|
|
|
consdt/=2.0;
|
|
if(distance>40.0)
|
|
{
|
|
mode = Linear;
|
|
}
|
|
|
|
break;
|
|
case Linear: // =================== Linear
|
|
// consigne distance
|
|
consds = distance / 4;
|
|
if(consds > vitesse) consds = vitesse;
|
|
|
|
// consigne angle
|
|
if(y-yf<=0)
|
|
{
|
|
if(x-xf<=0) angle = asin((yf-y)/distance);
|
|
else angle = -M_PI - asin((yf-y)/distance);
|
|
}
|
|
else
|
|
{
|
|
if(x-xf<=0) angle = asin((yf-y)/distance);
|
|
else angle = M_PI - asin((yf-y)/distance);
|
|
}
|
|
|
|
consdt = -angle-t;
|
|
if(consdt>M_PI) consdt-=2*M_PI;
|
|
if(consdt<-M_PI) consdt+=2*M_PI;
|
|
|
|
if(way==Back)
|
|
{
|
|
consds = -consds;
|
|
consdt=consdt+M_PI;
|
|
if(consdt>M_PI) consdt-=2*M_PI;
|
|
if(consdt<-M_PI) consdt+=2*M_PI;
|
|
}
|
|
|
|
if(distance<20.0)
|
|
{
|
|
mode = Fixe;
|
|
last_t = t;
|
|
}
|
|
else if(consdt>ANGLE_MAX || consdt<-ANGLE_MAX)
|
|
{
|
|
mode = Rotate;
|
|
xr=x;
|
|
yr=y;
|
|
if(way==Back) td = angle+M_PI; else td = angle;
|
|
}
|
|
consdt/=2.0;
|
|
|
|
break;
|
|
case Rotate: // =================== Rotate
|
|
// consigne distance
|
|
consds = 0;
|
|
if(consds > vitesse) consds = vitesse;
|
|
|
|
// consigne angle
|
|
consdt = -td-t;
|
|
if(consdt>M_PI) consdt-=2*M_PI;
|
|
if(consdt<-M_PI) consdt+=2*M_PI;
|
|
|
|
if(consdt<ANGLE_MAX & consdt>-ANGLE_MAX) mode = Linear;
|
|
|
|
break;
|
|
}
|
|
|
|
/*if(distance<30)
|
|
{
|
|
|
|
}
|
|
else
|
|
{
|
|
if(consdt>ANGLE_MAX || consdt<-ANGLE_MAX)
|
|
{
|
|
consds=0;
|
|
kt=2;
|
|
}
|
|
else kt=1;
|
|
last_t=consdt+t;
|
|
}*/
|
|
|
|
if(consdt>M_PI) consdt-=2*M_PI;
|
|
if(consdt<-M_PI) consdt+=2*M_PI;
|
|
|
|
|
|
// calcul consigne moteur
|
|
tand = (ENTRAX*tan(consdt))/(TICK1*20);
|
|
if(consdt>=M_PI/2) tand=vitesse;
|
|
if(consdt<=-M_PI/2) tand=-vitesse;
|
|
if(tand>vitesse/2) tand=vitesse/2;
|
|
if(tand<-vitesse/2) tand=-vitesse/2;
|
|
consV1 = consds - tand;
|
|
consV2 = consds + tand;
|
|
|
|
// pid moteur 1
|
|
errp1 = kp1 * (consV1 - v1);
|
|
errd1 = kd1 * ((consV1 - v1) - (consV1 - ancv1));
|
|
err1 = (short)errp1 + (short)errd1;
|
|
if(err1>4095) err1=4095;
|
|
if(err1<-4095) err1=-4095;
|
|
if(err1>-ERR_MINI && err1<ERR_MINI) err1=0;
|
|
|
|
// pid moteur 2
|
|
errp2 = kp2 * (consV2 - v2);
|
|
errd2 = kd2 * ((consV2 - v2) - (consV2 - ancv2));
|
|
err2 = (short)errp2 + (short)errd2;
|
|
if(err2>4095) err2=4095;
|
|
if(err2<-4095) err2=-4095;
|
|
if(err2>-ERR_MINI && err2<ERR_MINI) err2=0;
|
|
|
|
// anti marche arriere
|
|
if(consds>5.0)
|
|
{
|
|
if(err1<0) err1=0;
|
|
if(err2<0) err2=0;
|
|
}
|
|
if(consds<-5)
|
|
{
|
|
if(err1>0) err1=0;
|
|
if(err2>0) err2=0;
|
|
}
|
|
|
|
if(arret!=0)
|
|
{
|
|
m1 = 0;
|
|
AM1 = 1;
|
|
BM1 = 1;
|
|
|
|
m2 = 0;
|
|
AM2 = 1;
|
|
BM2 = 1;
|
|
}
|
|
else
|
|
{
|
|
if(err1>0)
|
|
{
|
|
BM1 = 0;
|
|
AM1 = 1;
|
|
m1 = (err1)+PWM_MINI;
|
|
}
|
|
else if(err1<0)
|
|
{
|
|
AM1 = 0;
|
|
BM1 = 1;
|
|
m1 = (err1*-1)+PWM_MINI;
|
|
}
|
|
else
|
|
{
|
|
AM1 = 1;
|
|
BM1 = 1;
|
|
m1 = 2048;
|
|
}
|
|
|
|
if(err2>0)
|
|
{
|
|
BM2 = 0;
|
|
AM2 = 1;
|
|
m2 = (err2)+PWM_MINI;
|
|
}
|
|
else if(err2<0)
|
|
{
|
|
AM2 = 0;
|
|
BM2 = 1;
|
|
m2 = (err2*-1)+PWM_MINI;
|
|
}
|
|
else
|
|
{
|
|
AM2 = 1;
|
|
BM2 = 1;
|
|
m2 = 2048;
|
|
}
|
|
}
|
|
|
|
time_flag=0;
|
|
|
|
if(cnt++>6)
|
|
{
|
|
if(id<600)
|
|
{
|
|
tab0[id]=(short)x;
|
|
tab1[id]=(short)err1;
|
|
tab2[id]=(short)err2;
|
|
tab3[id]=(short)v1;
|
|
tab4[id]=(short)v2;
|
|
id++;
|
|
}
|
|
cnt=0;
|
|
}
|
|
}
|
|
|
|
void asserv_deg(void)
|
|
{
|
|
|
|
|
|
while(time_flag==0);
|
|
|
|
consV2 = top_consigne - c2;
|
|
if(consV2>top_vitesse) consV2 = top_vitesse;
|
|
if(consV2<-top_vitesse) consV2 = -top_vitesse;
|
|
|
|
// pid moteur 1
|
|
errp1 = kp1 * (consV1 - v1);
|
|
errd1 = kd1 * ((consV1 - v1) - (consV1 - ancv1));
|
|
err1 = (short)errp1 + (short)errd1;
|
|
if(err1>4095) err1=4095;
|
|
if(err1<-4095) err1=-4095;
|
|
if(err1>-PWM_MINI && err1<PWM_MINI) err1=0;
|
|
|
|
// pid moteur 2
|
|
errp2 = kp2 * (consV2 - v2);
|
|
errd2 = kd2 * ((consV2 - v2) - (consV2 - ancv2));
|
|
err2 = (short)errp2 + (short)errd2;
|
|
if(err2>4095) err2=4095;
|
|
if(err2<-4095) err2=-4095;
|
|
if(err2>-PWM_MINI && err2<PWM_MINI) err2=0;
|
|
|
|
if(arret!=0)
|
|
{
|
|
m1 = 0;
|
|
m2 = 0;
|
|
}
|
|
else
|
|
{
|
|
if(err2>=0.0)
|
|
{
|
|
BM2 = 1;
|
|
AM2 = 0;
|
|
m2 = (short)(err2);
|
|
}
|
|
else
|
|
{
|
|
AM2 = 1;
|
|
BM2 = 0;
|
|
m2 = (short)(err2*-1);
|
|
}
|
|
}
|
|
|
|
time_flag=0;
|
|
}
|
|
|
|
#ifdef ASSERV_POS
|
|
void interrupt timer1_int(void) @ T1_VCTR
|
|
{
|
|
// calcul position
|
|
#ifdef BIG_BOT
|
|
dt = atan((v1-v2)*TICK1/ENTRAX);
|
|
ds = (v1 + v2)*HTICK;
|
|
#elif SMALL_BOT
|
|
dt = atan((v2-v1)*TICK1/ENTRAX);
|
|
ds = (v1 + v2)*HTICK;
|
|
#else
|
|
#error "no bot define"
|
|
#endif
|
|
x += ds * cos(t+dt*.5);
|
|
y -= ds * sin(t+dt*.5);
|
|
t -= dt;
|
|
if(t>M_PI) t-=2*M_PI;
|
|
if(t<-M_PI) t+=2*M_PI;
|
|
|
|
|
|
c1=POS1CNT;
|
|
c2=POS2CNT;
|
|
|
|
v1=c1-ancc1;
|
|
v2=c2-ancc2;
|
|
|
|
ancc1=c1;
|
|
ancc2=c2;
|
|
ancv1=v1;
|
|
ancv2=v2;
|
|
|
|
time_flag=1;
|
|
T1IF = 0;
|
|
}
|
|
#elif ASSERV_DEG
|
|
void interrupt timer1_int(void) @ T1_VCTR
|
|
{
|
|
c1=POS1CNT;
|
|
c2=POS2CNT;
|
|
v1=c1-ancc1;
|
|
v2=c2-ancc2;
|
|
|
|
ancc1=c1;
|
|
ancc2=c2;
|
|
time_flag=1;
|
|
T1IF = 0;
|
|
}
|
|
#else
|
|
#error "You have to define any asserv mode in configuration project"
|
|
#endif
|