|
#include "asserv.h"
|
|
|
|
#include <dspic.h>
|
|
#include <math.h>
|
|
#define M_PI 3.14159265358979323846
|
|
|
|
#include "setup.h"
|
|
|
|
//declaration des variables
|
|
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;
|
|
// declaration tableau
|
|
short tab0[600];
|
|
far short tab1[600], tab2[600]; // tableau de type far
|
|
far short ydata tab3[600], tab4[600]; // tableau de type far
|
|
|
|
extern unsigned char arret;
|
|
|
|
#define ANGLE_MAX M_PI/10
|
|
|
|
#ifdef BIG_BOT // si la constante BIT_BOT est d?finie on ex?cute le code si dessous
|
|
#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 // sinon on exc?cute les constantes si dessous
|
|
#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 // ASSERV_DEG est d?fini on exc?cute de code si dessous
|
|
// declaration des constante
|
|
#define ENTRAX 219.5
|
|
#define TICK1 0.0223993542529
|
|
#define HTICK 0.0111996771264
|
|
#define DELTAVITESSE 0.16
|
|
#define ERR_MINI 100
|
|
#define PWM_MINI 300
|
|
//declaration de variable
|
|
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 // sinon erreur
|
|
#error "no bot define"
|
|
#endif
|
|
|
|
/* la fonction setup_asserv permet d'ex?cuter les fonction setup_QEI1(); setup_QEI2();setup_PWM1();setup_timer1() et initialise les coordonn?e x et y
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
void setup_asserv()
|
|
{
|
|
setup_QEI1();
|
|
setup_QEI2();
|
|
setup_PWM1();
|
|
setup_timer1();
|
|
xf=x;
|
|
yf=y;
|
|
}
|
|
|
|
void setInitPos(unsigned short mx, unsigned short my)
|
|
/* fonction met a jour la position
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
{
|
|
x=mx/10.0;
|
|
y=my/10.0;
|
|
y=-y;
|
|
xf=x;
|
|
yf=y;
|
|
}
|
|
/*
|
|
fonction met a jour l'angle
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
void setInitTheta(unsigned short mt)
|
|
{
|
|
t=mt/1800.0*M_PI;
|
|
last_t=t;
|
|
}
|
|
|
|
void setDest(unsigned short mx, unsigned short my)
|
|
/*
|
|
fonction met a jour la destination
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
{
|
|
xf=mx/10.0;
|
|
yf=my/10.0;
|
|
yf=-yf;
|
|
}
|
|
|
|
void setThetaDest(unsigned short mt)
|
|
/*
|
|
fonction met a jour angle de destination
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
{
|
|
td=mt/1800.0*M_PI;
|
|
}
|
|
|
|
void setWay(Way mway)
|
|
/*fonction met a jour mway
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
{
|
|
way=mway;
|
|
}
|
|
/*
|
|
fonction met a jour le mode de commande
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
void setMode(Mode mmode)
|
|
{
|
|
mode=mmode;
|
|
}
|
|
/*
|
|
fonction met a jour la vitesse
|
|
la fonction est de type void il retourn rien
|
|
*/
|
|
void setVitesse(unsigned char mvitesse)
|
|
{
|
|
vitesse = mvitesse;
|
|
}
|
|
|
|
unsigned short getX()
|
|
/*
|
|
la fonction retourne la position x
|
|
*/
|
|
{
|
|
return (unsigned short)(x*10.0);
|
|
}
|
|
|
|
unsigned short getY()
|
|
{
|
|
return (unsigned short)(y*-10.0);
|
|
}
|
|
|
|
State getState()
|
|
/*
|
|
la fonction retourne les information sur l'etat (state)
|
|
*/
|
|
{
|
|
return state;
|
|
}
|
|
|
|
short getTheta()
|
|
/*
|
|
la fonction retourne l'angle theta
|
|
*/
|
|
{
|
|
return (short)(t*1800.0/M_PI);
|
|
}
|
|
/*
|
|
la fonction retourne la distance
|
|
*/
|
|
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;
|
|
}
|
|
/*
|
|
si power est superieur a zero on fait avancer le moteur ( soit gauche soit droite )
|
|
sinon on rend positive power puis on fait tourner le moteur dans le sens oppos?
|
|
*/
|
|
void setPower(short power)
|
|
{
|
|
if(power>=0)
|
|
{
|
|
m1=power;
|
|
BM1 = 0;
|
|
AM1 = 1;
|
|
}
|
|
else
|
|
{
|
|
m1=-power;
|
|
BM1 = 1;
|
|
AM1 = 0;
|
|
}
|
|
}
|
|
/*
|
|
la fonction retourne c2
|
|
*/
|
|
unsigned short getTop()
|
|
{
|
|
return c2;
|
|
}
|
|
/*
|
|
fonction d'asservissement du moteur
|
|
*/
|
|
void asserv(void)
|
|
{
|
|
while(time_flag==0); // tant que time_flag=0 on fait rien
|
|
|
|
// rampe vitesse
|
|
//if(vitesse<vitesseMax) vitesse+=DELTAVITESSE;
|
|
|
|
distance = sqrt((x-xf)*(x-xf)+(y-yf)*(y-yf)); // si time_flag est different de 0 on met a jour la distance
|
|
|
|
// FSM
|
|
switch(mode) // selon l'etat de mode
|
|
{
|
|
case Stop: // si mode=stop on met consds et consdt a 0 // =================== Stop
|
|
consds=0.0;
|
|
consdt=0.0;
|
|
break;
|
|
case Fixe: // =================== Fixe
|
|
// consigne distance
|
|
consds = distance / 4;//// si Fixe on met a consds
|
|
if(consds > vitesse) consds = vitesse; //si consds superieur a la vitesse consds prend la valeur de la vitesse
|
|
|
|
// consigne angle
|
|
// on met a jour la valeur de l'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
|
|
// si mode linear on met a jour consds et si elle est superieur a la vitesse elle prend la valeur de la vitesse
|
|
consds = distance / 4;
|
|
if(consds > vitesse) consds = vitesse;
|
|
|
|
// consigne angle
|
|
// on met a jour la valeur de l'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;// si consdt superieur a M_PI on decremente consdt
|
|
if(consdt<-M_PI) consdt+=2*M_PI; // si consdt inferieur a M_PI on incremente consdt
|
|
|
|
if(way==Back) // si way egale a back
|
|
{
|
|
consds = -consds;// consds prend son oppos?
|
|
consdt=consdt+M_PI;// consdt est mis a jour
|
|
if(consdt>M_PI) consdt-=2*M_PI; // si consdt est superieur M_Pi consdt est decremente de 2pi
|
|
if(consdt<-M_PI) consdt+=2*M_PI; // si consdt est inferieur de -2pi on decremente de 2*PI
|
|
}
|
|
|
|
if(distance<20.0) // si distance <20.0 on met le moteur en mode fixe et last_t prend la valeur de t
|
|
{
|
|
mode = Fixe;
|
|
last_t = t;
|
|
}
|
|
else if(consdt>ANGLE_MAX || consdt<-ANGLE_MAX)
|
|
/* sinon si consdt > a l'angle max ou consdt < a angle_max
|
|
le moteur est mis en mode rotation et on met a jour xr et yr
|
|
*/
|
|
|
|
|
|
{
|
|
mode = Rotate;
|
|
xr=x;
|
|
yr=y;
|
|
if(way==Back) td = angle+M_PI; else td = angle;// si way
|
|
}
|
|
consdt/=2.0;
|
|
|
|
break;
|
|
case Rotate: // =================== Rotate
|
|
// consigne distance
|
|
// si on est en mode rotation on met consds a 0
|
|
consds = 0;
|
|
if(consds > vitesse) consds = vitesse;// si consds superieur a la vitesse elle prend la valeur de la vitesse
|
|
|
|
// consigne angle
|
|
// mise a jour de consdt
|
|
consdt = -td-t;
|
|
|
|
if(consdt>M_PI) consdt-=2*M_PI; // si superieur a M_PI on decremente consdt de 2*M_PI
|
|
if(consdt<-M_PI) consdt+=2*M_PI; // si superieur a M_PI on incremente consdt de 2*M_PI
|
|
|
|
if(consdt<ANGLE_MAX & consdt>-ANGLE_MAX) mode = Linear; //si consdt est inferieur a Angle_Max et consdt superieur a - Angle_Max le moteur est mis en mode lineaire
|
|
|
|
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; // si superieur a M_PI on decremente consdt de 2*M_PI
|
|
if(consdt<-M_PI) consdt+=2*M_PI; // si inferieur a M_PI on decremente consdt de 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) //si consds n'est pas dans l'interval [-5,5] on met err1 et err2 a 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)// si le moteur n'est pas en arret on met a 0 le m1 et m2 et a 1 AM et BM des deux moteurs
|
|
{
|
|
m1 = 0;
|
|
AM1 = 1;
|
|
BM1 = 1;
|
|
|
|
m2 = 0;
|
|
AM2 = 1;
|
|
BM2 = 1;
|
|
}
|
|
else // sinon si elle est en arret on fait avancer les moteur selon la valeur de err1 et err2 puis on met a jour m1 et m2
|
|
{
|
|
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;
|
|
// on incremente le compteur et si elle est superieur a 6 et si id<600 on remplie les tableau de tab0 a tab4 et on incremente id a chaque fois que les tableau sont remplie
|
|
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++;
|
|
}
|
|
//si id>600 on met le compteur a 0
|
|
cnt=0;
|
|
}
|
|
}
|
|
/*
|
|
fonction d'asservissement de l'angle du moteur
|
|
*/
|
|
void asserv_deg(void)
|
|
{
|
|
|
|
|
|
while(time_flag==0); // tant que le flag=0 on fait rien
|
|
|
|
consV2 = top_consigne - c2;
|
|
if(consV2>top_vitesse) consV2 = top_vitesse; // si consV2 superieur a top_vitesse consV2 prend la valeur de top_vitesse
|
|
if(consV2<-top_vitesse) consV2 = -top_vitesse;// sinon elle prend l'oppos? de top_vitesse
|
|
|
|
// pid moteur 1
|
|
// mis a jour des valeur du 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
|
|
// mis a jour des valeurs du 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) // si arret est different de 0 on met le moteur m1 et m2 a zero
|
|
{
|
|
m1 = 0;
|
|
m2 = 0;
|
|
}
|
|
else
|
|
// sinon si err2 superieur a 0 on fait avancer le moteur 2
|
|
{
|
|
if(err2>=0.0)
|
|
{
|
|
BM2 = 1;
|
|
AM2 = 0;
|
|
m2 = (short)(err2); // m2 prend la valeur de err2 qui est converti en short
|
|
}
|
|
else
|
|
// sinon on fait avancer le moteur dans le sens oppos? puis m2 prend l'oppos? de err2
|
|
{
|
|
AM2 = 1;
|
|
BM2 = 0;
|
|
m2 = (short)(err2*-1);
|
|
}
|
|
}
|
|
|
|
time_flag=0;// on met time_flag a 0
|
|
}
|
|
|
|
#ifdef ASSERV_POS // si ASSERV_POS est d?finie on ex?cute l'interruption du timer1
|
|
void interrupt timer1_int(void) @ T1_VCTR
|
|
{
|
|
// calcul position
|
|
//T1CK1 : external clock input
|
|
#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; // interrupt flag status
|
|
}
|
|
#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
|