Brandon Modon
Prog_cate-Mbed - Reception de consigne à revoir -,,,
Dependencies: Asservissement Encoder_Nucleo_16_bits MYCRAC mbed
main.cpp
- Committer:
- Brand101
- Date:
- 2017-04-19
- Revision:
- 0:444ac4067b19
File content as of revision 0:444ac4067b19:
#include "mbed.h"
#include "Nucleo_Encoder_16_bits.h"
#include <math.h>
#include "MYCRAC_utility.h"
#include "Asservissement.h"
#include "global.h"
#include "MY_Asserv.h"
#include "My_ident.h"
//#include "CRAC_utility.h"
//------------------------ prototypes de fcts ------------------------------------
void Asser_Pos_Mot1(double pcons_pos);
void Asser_Pos_Mot2(double pcons_pos);
void Avancer_ligne_droite(long pcons, short vmax, short amax, short dmax);
void Avancer_ligne_droite_XYTheta(long pcons, short vmax, short amax, short dmax);
void Rotation(long pcons, short vmax, short amax, short dmax);
void Rotation_XYTheta(long pcons, short vmax, short amax, short dmax);
void X_Y_Theta(long px, long py, long ptheta, long sens, short vmax, short amax);
void Recalage(int pcons, short vmax, short amax, short dir, short nv_val);
void Rayon_De_Courbure(short rayon, short theta, short vmax, short amax, short sens, short dmax);
void Odometrie(void); // void Odometrie(void);
void Ralentir(short nouvelle_vitesse);
void Arret(void);
void canProcessRx(void);
void clacul_odo();
void canRx_ISR();
void MY_ISR();
Serial pc(SERIAL_TX, SERIAL_RX);
CAN can1(PB_8, PB_9);
CANMessage canmsg = CANMessage();
char message[8]= {0x00,0x01,0x10,0x11,0x00,0x01,0x10,0x11};
CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en r�ception pour le CAN
unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Ticker timer;
Ticker cal_odo;
//------------------------ def de var globales ------------------------------------
PwmOut mot1(PA_8);
PwmOut mot2(PA_5);
DigitalOut INA_M2(PC_6); //p37
DigitalOut INB_M2(PC_7); //p38
DigitalOut INA_M1(PB_0); //P26
DigitalOut INB_M1(PB_1); //P27
Nucleo_Encoder_16_bits encoder1(TIM3, 0xffff, TIM_ENCODERMODE_TI12);
Nucleo_Encoder_16_bits encoder2(TIM4, 0xffff, TIM_ENCODERMODE_TI12);
double consigne_pos, consigne_vit, // Consignes de position et de vitesse dans les mouvements
commande1, commande2, // Commande, aka duty cycle des PWM de chaque moteur
last_pos1, last_pos2, pos1, pos2, // Valeurs des compteurs incr�mentaux aux instants t et t-1
ErreurPos1, ErreurPos2, last_ErreurPos1, last_ErreurPos2, // Valeurs des erreurs de position de la boucle d'asservissement
Somme_ErreurPos1, Somme_ErreurPos2, // Valeurs des int�grales des erreurs
Delta_ErreurPos1, Delta_ErreurPos2, // Valeurs des d�riv�es des erreurs
Kpp1, Kip1, Kdp1, Kpp2, Kip2, Kdp2, // Valeurs des correcteurs d'asservissement pour les 2 moteurs
Kpp1a, Kip1a, Kdp1a, Kpp2a, Kip2a, Kdp2a, // Valeurs des correcteurs d'asservissement pour les 2 moteurs
VMAX, AMAX, DMAX, // Valeurs maximales d'acc�leration, d�c�l�ration et vitesse
Odo_x, Odo_y, Odo_theta, Odo_val_pos_1, Odo_val_pos_2, Odo_last_val_pos_1, Odo_last_val_pos_2; // Variables de positions utilis�es pour l'odom�trie
uint32_t roue_drt_init, roue_gch_init; // Valeur des compteurs (!= 0) quand on commence un nouveau mouvement
short etat_automate, etat_automate_depl, new_message,
xytheta_sens, next_move_xyt, next_move, i, cpt, stop, stop_receive, param_xytheta[3],
etat_automate_xytheta, ralentare, recalage_debut, couleur_debut, recalage_debut_receive;
char nb_ordres, vitesse_danger, Stop_Danger, cpt_ordre, asser_actif;
struct Ordre_deplacement liste[200];
uint16_t ms_count = 0, // Compteur utilis� pour envoyer �chantillonner les positions et faire l'asservissement
ms_count1 = 0, // Compteur utilis� pour envoyer la trame CAN d'odom�trie
flag_odo,
test3 = 0,
test4 = 0,
test5 = 0,
test6 =0,
test7 =0,
test8 =0;
double test1=0,
test2=0;
//------------------------ main ------------------------------------
int main()
{
pc.baud(230400);
pc.printf("Debut du prog");
INA_M2 = 0;
INB_M2 = 0;
INA_M1 = 0;
INB_M1 = 0;
mot1.period(1./20000.);
mot2.period(1./20000.);
can1.frequency(1000000);
consigne_pos = 0.0;
next_move = 0;
asser_actif=1;
cpt_ordre = 0;
nb_ordres = 1;
stop = 1;
stop_receive = 1;
ralentare = 0;
recalage_debut_receive = 0;
VMAX = 50;
AMAX = 125;
DMAX = 125;
roue_drt_init = (encoder1.GetCounter() + 20000);
roue_gch_init = COEF_ROUE_GAUCHE * ((double)encoder2.GetCounter() + 20000);
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
timer.attach(&MY_ISR, 0.001);
can1.attach(&canRx_ISR); // cr�ation de l'interrupt attach�e � la r�ception sur le CAN
cal_odo.attach(&clacul_odo, 0.01);
Kpp1 = 0.5;
Kip1 = 0.0001;
Kdp1 = 0.8;
Kpp2 = 0.5;
Kip2 = 0.0001;
Kdp2 = 0.8;
while(1) {
//TEST
//Asser_Pos_Mot1(0.0);
//Asser_Pos_Mot2(0.0);
//test3++;
canProcessRx();
if(flag_odo) {
flag_odo = 0;
Odometrie();
}
pc.printf("init_d:%d inti_g:%d dit:%hd t8:%d\n", roue_drt_init, roue_gch_init, liste[cpt_ordre].distance, test8);
//pc.printf("com1=%f com2=%f ePos1=%f ePos2=%f enc2=%d enc1=%d\n",commande1, commande2, ErreurPos1, ErreurPos2, encoder2.GetCounter(), encoder1.GetCounter());
//wait(0.001);
//pc.printf("t1=%d t2=%d t3=%d t4=%d t5=%d t6=%d t7=%d drt_init=%f g_init=%f \n", test1, test2, test3, test4, test5, test6, test7, roue_drt_init, roue_gch_init);
/*
write_PWM2(commande2);
if(ms_count==19) {
test3++;
}
*/
}
}
void clacul_odo()
{
flag_odo = 1;
}
void canRx_ISR()
{
if (can1.read(msgRxBuffer[FIFO_ecriture])) {
FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
/*
if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
*/
}
}
void canProcessRx(void)
{
static signed char FIFO_occupation=0,FIFO_max_occupation=0;
CANMessage msgTx=CANMessage();
FIFO_occupation=FIFO_ecriture-FIFO_lecture;
if(FIFO_occupation<0)
FIFO_occupation=FIFO_occupation+SIZE_FIFO;
if(FIFO_max_occupation<FIFO_occupation)
FIFO_max_occupation=FIFO_occupation;
if(FIFO_occupation!=0) {
switch(msgRxBuffer[FIFO_lecture].id) {
case ASSERVISSEMENT_INFO_CONSIGNE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_INFO_CONSIGNE);
Send_assev_info_comfig();
break;
case ASSERVISSEMENT_CONFIG_KPP_DROITE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPP_DROITE);
break;
case ASSERVISSEMENT_CONFIG_KPI_DROITE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPI_DROITE);
break;
case ASSERVISSEMENT_CONFIG_KPD_DROITE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPD_DROITE);
break;
case ASSERVISSEMENT_CONFIG_KPP_GAUCHE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPP_GAUCHE);
break;
case ASSERVISSEMENT_CONFIG_KPI_GAUCHE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPI_GAUCHE);
break;
case ASSERVISSEMENT_CONFIG_KPD_GAUCHE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPD_GAUCHE);
break;
case ASSERVISSEMENT_ENABLE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_ENABLE);
asser_actif = msgRxBuffer[FIFO_lecture].data[0];
break;
/*
case ODOMETRIE_SMALL_POSITION:
SendAck(ACKNOWLEDGE_MOTEUR, ODOMETRIE_SMALL_POSITION);
Odometrie(); // il envoie automatiquement d'odo apr�s calcule
break;
*/
case ODOMETRIE_SMALL_VITESSE:
SendAck(ACKNOWLEDGE_MOTEUR, ODOMETRIE_SMALL_VITESSE);
SendSpeed(consigne_pos, 0);
break;
case ASSERVISSEMENT_STOP:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_STOP);
stop_receive = 1;
liste[cpt_ordre].type = TYPE_DEPLACEMENT_STOP;
break;
case ASSERVISSEMENT_SPEED_DANGER: //
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_SPEED_DANGER);
break;
case ASSERVISSEMENT_XYT:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_XYT);
etat_automate_xytheta = 1;
liste[cpt_ordre].type = TYPE_DEPLACEMENT_X_Y_THETA;
liste[cpt_ordre].x = (msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8);
liste[cpt_ordre].y = (msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8);
liste[cpt_ordre].theta = (msgRxBuffer[FIFO_lecture].data[4]) + (msgRxBuffer[FIFO_lecture].data[5] << 8);
liste[cpt_ordre].sens = msgRxBuffer[FIFO_lecture].data[6];
break;
case ASSERVISSEMENT_COURBURE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_COURBURE);
liste[cpt_ordre].type = TYPE_DEPLACEMENT_RAYON_COURBURE;
liste[cpt_ordre].rayon = (msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8);
liste[cpt_ordre].theta_ray = (msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8);
liste[cpt_ordre].sens = msgRxBuffer[FIFO_lecture].data[4];
liste[cpt_ordre].enchainement = msgRxBuffer[FIFO_lecture].data[5];
break;
case ASSERVISSEMENT_CONFIG:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG);
VMAX = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
AMAX = ((msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8));
ralentare = 1;
break;
case ASSERVISSEMENT_ROTATION:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_ROTATION);
liste[cpt_ordre].type = TYPE_DEPLACEMENT_ROTATION;
liste[cpt_ordre].angle = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
Rotate (Odo_theta);
break;
case ASSERVISSEMENT_RECALAGE:
SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_RECALAGE);
liste[cpt_ordre].type = TYPE_DEPLACEMENT_RECALAGE;
liste[cpt_ordre].distance = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
liste[cpt_ordre].recalage = ((msgRxBuffer[FIFO_lecture].data[3]) + (msgRxBuffer[FIFO_lecture].data[4] << 8));
liste[cpt_ordre].val_recalage = msgRxBuffer[FIFO_lecture].data[2];
recalage_debut = 1;
recalage_debut_receive = 1;
break;
case ASSERVISSEMENT_LIGNE_DROITE: // n'existe pas encore
//SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_LIGNE_DROITE);
test8 = 10;
if(msgRxBuffer[FIFO_lecture].data[2] == 0) {
liste[cpt_ordre].type = TYPE_DEPLACEMENT_LIGNE_DROITE;
} else {
liste[cpt_ordre].type = TYPE_DEPLACEMENT_RECALAGE;
liste[cpt_ordre].recalage = ((msgRxBuffer[FIFO_lecture].data[3]) + (msgRxBuffer[FIFO_lecture].data[4] << 8));
}
recalage_debut_receive = msgRxBuffer[FIFO_lecture].data[2];
liste[cpt_ordre].distance = ((msgRxBuffer[FIFO_lecture].data[1] << 8) + (msgRxBuffer[FIFO_lecture].data[0]));
liste[cpt_ordre].enchainement = msgRxBuffer[FIFO_lecture].data[5];
//GoStraight(); /////////////////////////////////
break;
case MOTOR_IMOBILE:
SendAck(ACKNOWLEDGE_MOTEUR, MOTOR_IMOBILE);
liste[cpt_ordre].type = TYPE_DEPLACEMENT_IMMOBILE;
break;
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
}
}
void Asser_Pos_Mot1(double pcons_pos)
{
pos1 = (double)encoder1.GetCounter();
ErreurPos1 = pcons_pos - pos1;
Delta_ErreurPos1 = ErreurPos1 - last_ErreurPos1;
Somme_ErreurPos1 += ErreurPos1;
if(Somme_ErreurPos1>1.0) {
Somme_ErreurPos1=1.0;
} else if(Somme_ErreurPos1<-1.0) {
Somme_ErreurPos1=-1.0;
}
commande1 = (Kpp1 * ErreurPos1 + Kip1 * Somme_ErreurPos1 + Kdp1 * Delta_ErreurPos1);
last_ErreurPos1 = ErreurPos1;
last_pos1 = pos1;
commande1=commande1/2500.;
if(commande1>1.0) {
commande1=1.0;
} else if(commande1<-1.0) {
commande1=-1.0;
}
//test5++;
}
void Asser_Pos_Mot2(double pcons_pos)
{
pos2 = (double)encoder2.GetCounter();
ErreurPos2 = pcons_pos - pos2;
Delta_ErreurPos2 = ErreurPos2 - last_ErreurPos2;
Somme_ErreurPos2 += ErreurPos2;
if(Somme_ErreurPos2>1.0)
Somme_ErreurPos2=1.0;
else if(Somme_ErreurPos2<-1.0)
Somme_ErreurPos2=-1.0;
commande2 = (Kpp2 * ErreurPos2 + Kip2 * Somme_ErreurPos2 + Kdp2 * Delta_ErreurPos2);
last_ErreurPos2 = ErreurPos2;
last_pos2 = pos2;
commande2=commande2/2500.;
if(commande2>1.0) {
commande2=1.0;
} else if(commande2<-1.0) {
commande2=-1.0;
}
test6++;
}
void Avancer_ligne_droite(long pcons, short vmax, short amax, short dmax)
{
//Declaration des variables
static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
if(pcons >= 0) { //Mode avancer
switch(etat_automate_depl) { //Automate de gestion du deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//Remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
consigne_vit = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps � vitesse constante en fonction de l'acceleration
tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
// Le prochain �tat de l'automate sera l'�tat 5
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de vitesse par la valeur de l'acceleration
consigne_vit += amax;
//Incrementation de la consigne de position
consigne_pos += ((double)consigne_vit / 1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) { //Condition pour sortir de l'etat arret
etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
next_move = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de l'etat acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
}
else if(pcons < 0) { //Mode reculer
switch(etat_automate_depl) { //Automate de gestion du deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//Remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps � vitesse constante en fonction de l'acceleration
tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour sortir de l'etat de vitesse constante en profil trapeze
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour sortir de l'etat deceleration en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) { //Condition pour sortir de l'etat arret
etat_automate_depl = INITIALISATION; //Passage a l'etat d'INITIALISATION
next_move = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de l'etat d'acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
}
//Calcul des commande 1 et 2
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init + consigne_pos);
//Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
write_PWM2(commande2);
write_PWM1(commande1);
}
void Avancer_ligne_droite_XYTheta(long pcons, short vmax, short amax, short dmax)
{
//Declaration des variables
//char val1[8];
static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
if(pcons >= 0) { //Mode avancer
switch(etat_automate_depl) { //Automate de gestion du deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps a vitesse constante en fonction de l'acceleration
tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
cpt ++;
//Incrementation de la consigne de vitesse par la valeur de l'acceleration
consigne_pos += vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de vitesse constante en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INITIALISATION; //Passage a l'etat d'INITIALISATION
next_move_xyt = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
} else if(pcons < 0) { //Mode reculer
switch(etat_automate_depl) { //Automate de gestion du deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//Remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps � vitesse constante en fonction de l'acceleration
tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
} else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour sortir de l'etat de vitesse constante en profil trapeze
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour sortir de l'etat vitesse continue en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
next_move_xyt = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
}
//Calcul des commande 1 et 2
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init + consigne_pos);
//Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
write_PWM2(commande2);
write_PWM1(commande1);
}
void Rotation(long pcons, short vmax, short amax, short dmax)
{
//Declaration des variables
//char val1[8];
static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
if(pcons >= 0) { //Mode avancer
switch(etat_automate_depl) { //Automate de gestion de deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps a vitesse constante en fonction de l'acceleration
tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat AACELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
next_move = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0); //Condition pour sortir de la phase d'acceleration en profil triangle
if((cpt * amax / 1000.0) >= vmax_tri) {
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
} else if(pcons < 0) { //Mode reculer
switch(etat_automate_depl) { //Automate de gestion du deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//Remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps � vitesse constante en fonction de l'acceleration
tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour sortir de l'etat vitesse continue en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
next_move = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
}
//Calcul des commande 1 et 2
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init - consigne_pos);
//Ecriture du PWM sur chaque modeur /*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
write_PWM2(commande2);
write_PWM1(commande1);
}
void Rotation_XYTheta(long pcons, short vmax, short amax, short dmax)
{
//Declaration des variables
//char val1[8];
static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
if(pcons >= 0) { //Mode avancer
switch(etat_automate_depl) { //Automate de gestion de deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps a vitesse constante en fonction de l'acceleration
tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat AACELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
next_move_xyt = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0); //Condition pour sortir de la phase d'acceleration en profil triangle
if((cpt * amax / 1000.0) >= vmax_tri) {
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
} else if(pcons < 0) { //Mode reculer
switch(etat_automate_depl) { //Automate de gestion du deplacement
case INITIALISATION : //Etat d'initialisation et de calcul des variables
//Remise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Elaboration du profil de vitesse
//Calcul du temps � vitesse constante en fonction de l'acceleration
tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
}
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
}
break;
case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
cpt = 0;
}
break;
case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
cpt = 0;
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour sortir de l'etat vitesse continue en profil trapeze
etat_automate_depl = ARRET; //Passage a l'etat ARRET
consigne_pos = pcons;
cpt = 0;
}
break;
case ARRET : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
next_move_xyt = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
cpt = 0;
}
break;
case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
cpt ++;
//Decrementation de la consigne de position
consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
etat_automate_depl = ARRET; //Passage a l'etat ARRET
cpt = 0;
}
break;
}
}
//Calcul des commande 1 et 2
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init - consigne_pos);
//Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
write_PWM2(commande2);
write_PWM1(commande1);
}
void X_Y_Theta(long px, long py, long ptheta, long sens, short vmax, short amax)
{
//Variables pr�sentes Odo_x, Odo_y : position de d�part
//Declaration des variables
static short dist = 0, ang1 = 0, ang2 = 0;
//shar val[4];
switch(etat_automate_xytheta) {
case INIT_X_Y_THETA : //etat INIT_X_Y_THETA
//Mode Avancer
if(sens >= 0) {
/*pour avancer, on tourne dans le sens horaire//MOI*\*/
// Son hypoth�nuse correspond � la distance � parcourir
dist = (short)sqrt((px - Odo_x)*(px - Odo_x)+(py - Odo_y)*(py - Odo_y));
// la 1ere rotation correspond � l'angle du triangle, moins l'angle de la position de d�part
// C'est-�-dire la tangente du c�t� oppos� sur l'angle adjacent
// La fonction atan2 fait le calcul de la tangente en radians, entre Pi et -Pi
// On rajoute des coefficients pour passer en degr�s
// On ajoute 7200 dixi�mes de degr�s pour �tre s�rs que le r�sultat soit positif
ang1 = (short)((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta + 7200) % 3600;
// On passe le r�sultat entre -1800 et 1800
if(ang1 > 1800) ang1 = (ang1 - 3600);
// La 2� rotation correspond � l'angle de destination, moins l'angle � la fin de la ligne droite,
// donc le m�me qu'� la fin de la 1�re rotation, donc l'angle de d�part plus la premi�re rotation
// On ajoute 3600 pour �tre s�r d'avoir un r�sultat positif
ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
// On passe le r�sultat entre -1800 et 1800
if(ang2 > 1800) ang2 = (ang2 - 3600);
// On transforme les r�sultats en distance et angles utilisables avec les fonctions d�j� d�finies
dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
}
//Mode Reculer
else if(sens < 0) {
/*pour avancer, on tourne dans le sens anti-horaire//MOI*\*/
// Idem qu'au-dessus, mais laligne droite doit �tre faite en marche arri�re
// La distance est l'oppos� de celle calcul�e au dessus
// Les angles sont les m�mes � 1800 pr�s
dist = -(short)sqrt((px-Odo_x)*(px-Odo_x)+(py-Odo_y)*(py-Odo_y));
//Premiere rotation
ang1 = (short)(((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta) + 5400) % 3600;
if(ang1 > 1800) ang1 = (ang1 - 3600);
//Deuxieme rotation
ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
if(ang2 > 1800) ang2 = (ang2 - 3600);
// On transforme les r�sultats en distance et angles utilisables avec les fonctions d�j� d�finies
dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
}
param_xytheta[0] = ang1;
param_xytheta[1] = dist;
param_xytheta[2] = ang2;
// La fonction xyth�ta utilise un automate propre, similaire � l'automate g�n�ral
etat_automate_xytheta = 1;
etat_automate_depl = ROTATION_X_Y_THETA_1; //Passage a l'etat ROTATION_X_Y_THETA_1
break;
case ROTATION_X_Y_THETA_1 : //etat ROTATION_X_Y_THETA_1
//Execution de la fonction de rotation pour la fonction de deplacement X_Y_THETA
Rotation_XYTheta(ang1, vmax, amax, DMAX);
if(next_move_xyt) {
roue_drt_init = encoder1.GetCounter();
//#elif GROS_ROB == 0
roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//#endif
next_move_xyt = 0;
etat_automate_xytheta = LIGNE_DROITE_X_Y_THETA; //Passage a l'etat LIGNE_DROITE_X_Y_THETA
}
break;
case LIGNE_DROITE_X_Y_THETA : //etat LIGNE_DROITE_X_Y_THETA_1
//Execution de la fonction de ligne droite pour la fonction de deplacement X_Y_THETA
Avancer_ligne_droite_XYTheta(dist, vmax, amax, DMAX);
if(next_move_xyt) {
roue_drt_init = encoder1.GetCounter();
//#elif GROS_ROB == 0
roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//#endif
next_move_xyt = 0;
etat_automate_xytheta = ROTATION_X_Y_THETA_2; //Passage a l'etat ROTATION_X_Y_THETA_2
}
break;
case ROTATION_X_Y_THETA_2 : //etat ROTATION_X_Y_THETA_2
//Execution de la fonction de rotation pour la fonction de deplacement X_Y_THETA
Rotation_XYTheta(ang2, vmax, amax, DMAX);
if(next_move_xyt) {
roue_drt_init = encoder1.GetCounter();
//#elif GROS_ROB == 0
roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//#endif
next_move_xyt = 0;
etat_automate_xytheta = INIT_X_Y_THETA; //Passage a l'etat INIT_X_Y_THETA
next_move = 1;
}
break;
default : //Etat par defaut, on fait la meme chose que dans l'etat d'initialisation
//Mode Avancer
if(sens >= 0) {
/*pour avancer, on tourne dans le sens horaire//MOI*\*/
dist = (short)sqrt((px - Odo_x)*(px - Odo_x)+(py - Odo_y)*(py - Odo_y));
ang1 = (short)((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta + 7200) % 3600;
if(ang1 > 1800)
ang1 = (ang1 - 3600);
ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
if(ang2 > 1800)
ang2 = (ang2 - 3600);
dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
}
//Mode Reculer
else if(sens < 0) {
/*pour avancer, on tourne dans le sens anti-horaire//MOI*\*/
dist = -(short)sqrt((px-Odo_x)*(px-Odo_x)+(py-Odo_y)*(py-Odo_y));
ang1 = (short)(((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta) + 5400) % 3600;
if(ang1 > 1800)
ang1 = (ang1 - 3600);
ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
if(ang2 > 1800)
ang2 = (ang2 - 3600);
dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
}
param_xytheta[0] = ang1;
param_xytheta[1] = dist;
param_xytheta[2] = ang2;
etat_automate_xytheta = 1;
etat_automate_depl = INIT_X_Y_THETA;
break;
}
}
void Odometrie(void) //void Odometrie(void)
{
//Declaration des variables
//char val1[8];
double dist, ang; //distance, angle
//Recuperation des valeurs des compteurs des encodeurs
Odo_val_pos_1 = (double)encoder1.GetCounter();
/* MOI*\
//#elif GROS_ROB == 0
Odo_val_pos_2 = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//#endif
*/
Odo_val_pos_2 = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//Calcul de la distance parcourue
dist = 0.5*((Odo_val_pos_1 - Odo_last_val_pos_1) + (Odo_val_pos_2 - Odo_last_val_pos_2));
//Calcul de la valeur de l'angle parcouru
ang = ((Odo_val_pos_1 - Odo_last_val_pos_1) -(Odo_val_pos_2 - Odo_last_val_pos_2))*1800.0*PERIMETRE_ROUE_CODEUSE/(LARGEUR_ROBOT*PI*RESOLUTION_ROUE_CODEUSE);
//Determination de la position sur le terrain en X, Y, Theta
Odo_theta += ang;
Odo_x += dist*cos((double)(Odo_theta*PI/1800.0))*PERIMETRE_ROUE_CODEUSE/RESOLUTION_ROUE_CODEUSE;
Odo_y += dist*sin((double)(Odo_theta*PI/1800.0))*PERIMETRE_ROUE_CODEUSE/RESOLUTION_ROUE_CODEUSE;
//Stockage de la derniere valeur de l'odometrie
Odo_last_val_pos_1 = Odo_val_pos_1;
Odo_last_val_pos_2 = Odo_val_pos_2;
//Condition d'envoi des informations de l'odometrie par CAN
Send_odometrie();
/****************************************/
/****************************************/
/* <DEBUG> */
/****************************************/
/****************************************/
Debug_Asserv();
/****************************************/
/****************************************/
/* <DEBUG> */
/****************************************/
/****************************************/
}
void Ralentir(short nouvelle_vitesse)
{
//Si on allait en avant (cpt >= 0), on decremente cpt jusqu'a la nouvelle vitesse
if(cpt >= 0) {
cpt --;
if(cpt <= ((double)nouvelle_vitesse / ((double)AMAX/1000.0))) { //La vitesse souhaitait est atteinte
cpt = (short)((double)nouvelle_vitesse / ((double)AMAX/1000.0));
ralentare = 0;
}
}
//Si on allait en arri�re (cpt < 0), on incr�mente cpt jusqu'a la nouvelle vitesse
else if(cpt < 0) {
cpt ++;
if(cpt >= -((double)nouvelle_vitesse / ((double)AMAX/1000.0))) { //La vitesse souhaitait est atteinte
cpt = (short)-((double)nouvelle_vitesse / ((double)AMAX/1000.0));
ralentare = 0;
}
}
//Ralentir fonctionne comme une acc�l�ration standard, donc on modifie la consigne de position
consigne_pos += (double)(((double)cpt * (double)AMAX)/1000.0);
//Calcul de la valeur de commande 1 et 2
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init + consigne_pos);
//Ecriture de la valeur de la commande souhait�e
write_PWM2(commande2);
write_PWM1(commande1);
}
void Arret(void)
{
//Recuperation de la valeur de l'odometrie de la roue de droite et de gauche
roue_drt_init = (double)encoder1.GetCounter();
/* MOI*\
//#elif GROS_ROB == 0
roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//#endif
*/
roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//Calcul de la valeur des commandes 1 et 2
Asser_Pos_Mot1(roue_drt_init);
Asser_Pos_Mot2(roue_gch_init);
//Ecrire un PWM egal a 0
write_PWM2(0.0);
write_PWM1(0.0);
next_move = 1;
}
void Recalage(int pcons, short vmax, short amax, short dir, short nv_val)
{
//char val1[8];
//Mode AVANCER
if(pcons >= 0) {
switch(etat_automate_depl) {
case INIT_RECALAGE : //etat INIT_RECELAGE
etat_automate_depl = ACCELERATION_RECALAGE; //Passage a l'etat ACCELERATION_RECALAGE
consigne_pos = 0; //Initialisation des differentes variables
cpt = 0;
break;
case ACCELERATION_RECALAGE : //etat ACCELERATION_RECALAGE
// Phase d'acc�leration comme si on faisait une ligne droite
if(consigne_pos >= pcons/2) etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
cpt ++;
//Calcul de la consigne de position
consigne_pos += (((double)cpt * (double)amax)/1000.0);
if(cpt >= ((double)vmax / ((double)amax/1000.0))) {
etat_automate_depl = VITESSE_CONSTANTE_RECALAGE; //Passage a l'etat VITESSE_CONSTANTE_RECALAGE
}
// Si les 2 erreurs sont sup�rieures � un seuil d�fini, on a fini le recalage
if(ErreurPos1 >= RECALAGE_TH && ErreurPos2 >= RECALAGE_TH) {
etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
commande1 = 0.0; //Remise a zero des valeurs des commandes
commande2 = 0.0;
}
// Si on d�tecte une augmentation d'erreur sur une roue, on l'arr�te pour �viter de patiner
else if(ErreurPos1 >= RECALAGE_TH) commande1 = 0.0;
else if(ErreurPos2 >= RECALAGE_TH) commande2 = 0.0;
break;
case VITESSE_CONSTANTE_RECALAGE : //etat VITESSE_CONSTANTE_RECALAGE
// Phase de vitesse constante
consigne_pos += vmax;
// Idem que plus haut, on surveille les erreurs des 2 roues
if(ErreurPos1 >= RECALAGE_TH && ErreurPos2 >= RECALAGE_TH) {
etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
commande1 = 0.0; //Remise a zero des valeurs des commandes
commande2 = 0.0;
} else if(ErreurPos1 >= RECALAGE_TH) commande1 = 0.0; //Mise a zero de la commande 1
else if(ErreurPos2 >= RECALAGE_TH) commande2 = 0.0; //Mise a zero de la commande 2
break;
case FIN_RECALAGE : //etat FIN_RECALAGE
// Fin du recalage, on met � jour les donn�es de position
if(cpt >=20) {
//Recalage de x
if(dir == 1) {
Odo_x = nv_val;
// On met l'angle � jour en fonction de la position sur le terrain
// Si on est dans la partie haute ( > la moiti�), on est dans un sens,
// Si on est dans la partie basse, on met � jour dans l'autre sens
// On prend aussi en compte le sens dans lequel on a fait la ligne droite
if(nv_val > 1000) {
if(pcons >=0)
Odo_theta = 0;
else
Odo_theta = 1800;
}
else {
if(pcons >= 0)
Odo_theta = 1800;
else
Odo_theta = 0;
}
}
//Recalage de y
else {
Odo_y = nv_val;
if(nv_val > 1500) {
if(pcons >=0)
Odo_theta = 900;
else
Odo_theta = -900;
}
else {
if(pcons >= 0)
Odo_theta = -900;
else
Odo_theta = 900;
}
}
etat_automate_depl = INIT_RECALAGE;
next_move = 1;
//Mise a zero des commandes et des moteurs;
commande1 = 0.0;
commande2 = 0.0;
write_PWM1(0.0);
write_PWM2(0.0);
}
cpt ++;
break;
}
}
//Mode RECULER
else if(pcons < 0) {
switch(etat_automate_depl) {
case INIT_RECALAGE : //etat INIT_RECELAGE
etat_automate_depl = ACCELERATION_RECALAGE; //Passage a l'etat ACCELERATION_RECALAGE
consigne_pos = 0; //Initialisation des diff�rentes variables
cpt = 0;
break;
case ACCELERATION_RECALAGE : //etat ACCELERATION_RECALAGE
// Phase d'acc�leration comme si on faisait une ligne droite
if(consigne_pos <= pcons/2) etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
cpt --;
consigne_pos += (double)(((double)cpt * (double)amax)/1000.0);
if(cpt <= -((double)vmax / ((double)amax/1000.0))) {
etat_automate_depl = VITESSE_CONSTANTE_RECALAGE; //Passage a l'etat VITESSE_CONSTANTE_RECALAGE
}
// Si les 2 erreurs sont inf�rieures � un seuil d�fini, on a fini le recalage
if(ErreurPos1 <= -RECALAGE_TH && ErreurPos2 <= -RECALAGE_TH) {
etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
commande1 = 0.0;
commande2 = 0.0;
}
// Si on d�tecte une augmentation d'erreur sur une roue, on l'arr�te pour �viter de patiner
else if(ErreurPos1 <= -RECALAGE_TH) commande1 = 0.0;
else if(ErreurPos2 <= -RECALAGE_TH) commande2 = 0.0;
break;
case VITESSE_CONSTANTE_RECALAGE : //etat VITESSE_CONSTANTE_RECALAGE
// Phase de vitesse constante
consigne_pos -= vmax;
// Idem que plus haut, on surveille les erreurs des 2 roues
if(ErreurPos1 <= -RECALAGE_TH && ErreurPos2 <= -RECALAGE_TH) {
etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
commande1 = 0.0;
commande2 = 0.0;
} else if(ErreurPos1 <= -RECALAGE_TH) commande1 = 0.0;
else if(ErreurPos2 <= -RECALAGE_TH)commande2 = 0.0;
break;
case FIN_RECALAGE : //etat FIN_RECALAGE
if(cpt >=20) {
//Recalage de x
if(dir == 1) {
Odo_x = nv_val;
if(nv_val > 1000) {
if(pcons >=0)
Odo_theta = 0;
else
Odo_theta = 1800;
}
else {
if(pcons >= 0)
Odo_theta = 1800;
else
Odo_theta = 0;
}
}
//Recalage de y
else {
Odo_y = nv_val;
if(nv_val > 1500) {
if(pcons >=0)
Odo_theta = 900;
else
Odo_theta = -900;
}
else {
if(pcons >= 0)
Odo_theta = -900;
else
Odo_theta = 900;
}
}
etat_automate_depl = 0;
next_move = 1;
//Mise a zero des commandes et des moteurs
commande1 = 0.0;
commande2 = 0.0;
write_PWM1(0.0);
write_PWM2(0.0);
}
cpt ++;
break;
}
}
//Calcul des correcteurs a apporter aux moteurs
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init + consigne_pos);
//Envoi de la nouvelle valeur de commande aux PWM
write_PWM2(commande2);
write_PWM1(commande1);
}
void Rayon_De_Courbure(short rayon, short theta, short vmax, short amax, short sens, short dmax)
{
//Declaration des variables
//char val1[8];
static double tc, ta, td, vmax_tri, pcons;
//Si l'angle a parcourir est positif
if(theta >= 0) {
switch(etat_automate_depl) {
case INIT_RAYON_COURBURE : //etat d'initialisation
etat_automate_depl = ACCELERATION_TRAPEZE_RAYON_COURBURE; //Passage a l'etat ACCELERATION_TRAPEZE_RAYON_COURBURE
//Mise a zero des variables car on effectue un nouveau deplacement
consigne_pos = 0;
cpt = 0;
//Calcul de la position voulue
pcons = ((rayon + LARGEUR_ROBOT/2) * 2 * PI * theta / 3600) * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
//Elaboration du profil de vitesse
//Calcul du temps a vitesse constante
tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
etat_automate_depl = ACCELERATION_TRAPEZE_RAYON_COURBURE; //Passage a l'etat ACCELERATION_TRAPEZE_RAYON_COURBURE
ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
canmsg.id = 0x7F1;
canmsg.len = 0;
can1.write(canmsg);
}
//Temps a vitesse constant inexistant
else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
etat_automate_depl = ACCELERATION_TRIANGLE_RAYON_COURBURE; //Passage a l'etat ACCELERATION_TRIANGLE_RAYON_COURBURE
//Calcul de la vitesse maximale en profil triangle
vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
canmsg.id = 0x7F5;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case ACCELERATION_TRAPEZE_RAYON_COURBURE : //Etat d'acceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE
cpt = 0;
canmsg.id = 0x7F2;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE : //Etat de vitesse constante en profil trapeze
cpt ++;
//Incrementation de la consigne de vitesse par la valeur de la vitesse
consigne_pos += vmax;
//Si il n'y a pas d'enchainements
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
etat_automate_depl = DECELERATION_TRAPEZE_RAYON_COURBURE; //Passage a l'etat DECELERATION_TRAPEZE_RAYON_COURBURE
cpt = 0;
canmsg.id = 0x7F3;
canmsg.len = 0;
can1.write(canmsg);
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case DECELERATION_TRAPEZE_RAYON_COURBURE : //Etat de deceleration en profil trapeze
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
etat_automate_depl = ARRET_RAYON_COURBURE; //Passage a l'etat ARRET_RAYON_COURBURE
consigne_pos = pcons;
cpt = 0;
canmsg.id = 0x7F4;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case ARRET_RAYON_COURBURE : //Etat d'arret
if(cpt >= 20) {
etat_automate_depl = INIT_RAYON_COURBURE; //Passage a l'etat d'INIT_RAYON_COURBURE
next_move = 1;
}
cpt ++;
break;
case ACCELERATION_TRIANGLE_RAYON_COURBURE : //Etat d'acceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
etat_automate_depl = DECELERATION_TRIANGLE_RAYON_COURBURE; //Passage a l'etat DECELERATION_TRIANGLE_RAYON_COURBURE
cpt = 0;
canmsg.id = 0x7F6;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case DECELERATION_TRIANGLE_RAYON_COURBURE : //Etat de deceleration en profil triangle
cpt ++;
//Incrementation de la consigne de position
consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
etat_automate_depl = ARRET_RAYON_COURBURE;
cpt = 0;
canmsg.id = 0x7F4;
canmsg.len = 0;
can1.write(canmsg);
}
break;
}
}
//Si l'angle a parcourir est negatif
else if(theta < 0) {
switch(etat_automate_depl) {
case 0 :
etat_automate_depl++;
consigne_pos = 0;
cpt = 0;
pcons = ((rayon + LARGEUR_ROBOT/2) * 2 * PI * theta / 3600) * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
if (tc > 0) {
etat_automate_depl++;
ta = (((double)vmax * 1000.0) / (double)amax);
td = (((double)vmax * 1000.0) / (double)dmax);
canmsg.id = 0x7F1;
canmsg.len = 0;
can1.write(canmsg);
} else if(tc < 0) {
etat_automate_depl = 5;
vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
canmsg.id = 0x7F5;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case 1 :
cpt ++;
consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
if(cpt >= ta) {
etat_automate_depl ++;
cpt = 0;
canmsg.id = 0x7F2;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case 2 :
cpt ++;
consigne_pos -= vmax;
if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
etat_automate_depl ++;
cpt = 0;
canmsg.id = 0x7F3;
canmsg.len = 0;
can1.write(canmsg);
}
//Si on est dans un enchainement, on passe � l'instruction suivante
else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
consigne_pos = pcons;
cpt_ordre = 0;
}
break;
case 3 :
cpt ++;
consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
if(cpt >= td) {
etat_automate_depl ++;
consigne_pos = pcons;
cpt = 0;
canmsg.id = 0x7F4;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case 4 :
if(cpt >= 20) {
etat_automate_depl = 0;
next_move = 1;
}
cpt ++;
break;
case 5 :
cpt ++;
consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
if((cpt * amax / 1000.0) >= vmax_tri) {
etat_automate_depl ++;
cpt = 0;
canmsg.id = 0x7F6;
canmsg.len = 0;
can1.write(canmsg);
}
break;
case 6 :
cpt ++;
consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {
etat_automate_depl = 4;
cpt = 0;
canmsg.id = 0x7F4;
canmsg.len = 0;
can1.write(canmsg);
}
break;
}
}
//Determination des commandes en fonction de la direction de deplacement du robot
if(sens >= 0) {
Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Asser_Pos_Mot2(roue_gch_init + (consigne_pos * ((2 * rayon) - LARGEUR_ROBOT)/((2 * rayon) + LARGEUR_ROBOT)));
} else if(sens < 0) {
Asser_Pos_Mot1(roue_drt_init + (consigne_pos * ((2 * rayon) - LARGEUR_ROBOT)/((2 * rayon) + LARGEUR_ROBOT)));
Asser_Pos_Mot2(roue_gch_init + consigne_pos);
}
//Envoi de la vitesse aux moteurs
write_PWM2(commande2);
write_PWM1(commande1);
}
void MY_ISR()
{
char val1[8];
if(ms_count == 20) {
//pc.printf("vit1=%f vit2=%f\n", test1, test2);
if(asser_actif) {
//Si on a re�u l'instruction de stop...
if(stop_receive) {
stop_receive = 0;
ErreurPos1 = 0;
ErreurPos2 = 0;
Somme_ErreurPos1 = 0;
Somme_ErreurPos2 = 0;
Delta_ErreurPos1 = 0;
Delta_ErreurPos2 = 0;
//On poursuit l'action si on est en rotation
if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_ROTATION);
//On s'arr�te tout de suite si on est en ligne droite
else if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_IMMOBILE || liste[cpt_ordre].type == TYPE_DEPLACEMENT_LIGNE_DROITE || liste[cpt_ordre].type == TYPE_DEPLACEMENT_RAYON_COURBURE) {
//On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
nb_ordres = 0;
etat_automate_depl = 0;
cpt_ordre = 0;
}
//Si on est en X_Y_Theta, on poursuit la rotation, ou on arr�te la ligne droite
else if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_X_Y_THETA) {
if(etat_automate_xytheta == 2) {
//On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
nb_ordres = 0;
cpt_ordre = 0;
}
}
}
//Si on a re�u un changement de vitesse...
else if(ralentare) {
Ralentir(VMAX);
for(i = 0; i<nb_ordres; i++) {
liste[i].vmax = VMAX;
liste[i].amax = AMAX;
}
}
else if(recalage_debut_receive) {
if(couleur_debut == 1) {
//#if GROS_ROB == 0
Odo_x = 1961;
Odo_y = 2800;
Odo_theta = 1800;
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(971 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
};
liste[1] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
};
liste[2] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_RECALAGE,0,30,500,(-1000 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,2,2961,0,0,0,0,0,0,0,0
};
liste[3] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(111 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
};
liste[4] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
};
recalage_debut_receive = 0;
} else if(couleur_debut == 0) {
//#if GROS_ROB == 0
Odo_x = 1961;
Odo_y = 200;
Odo_theta = 1800;
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(971 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
};
liste[1] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,-900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
};
liste[2] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_RECALAGE,0,30,500,(-1000 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,2,39,0,0,0,0,0,0,0,0
};
liste[3] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(111 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
};
liste[4] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,-900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
};
recalage_debut_receive = 0;
}
}
//Si on vient de finir un mouvement...
else if(next_move) {
ErreurPos1 = 0;
ErreurPos2 = 0;
Somme_ErreurPos1 = 0;
Somme_ErreurPos2 = 0;
Delta_ErreurPos1 = 0;
Delta_ErreurPos2 = 0;
//Si on devait s'arr�ter...
if(stop) {
//On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
nb_ordres = 0;
cpt_ordre = 0;
stop = 0;
next_move = 0;
cpt = 0;
//On pr�vient qu'on s'est arr�t�
motor_of();
}
//Sinon...
else if(!stop) {
next_move = 0;
//On conserve la position du robot pour la prochaine action
roue_drt_init = encoder1.GetCounter();
//MOI*\*/ // bug Edouard
//#elif GROS_ROB == 0
roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
//#endif
cpt_ordre++;
if(recalage_debut == 1) {
if(cpt_ordre >= 5) {
recalage_debut = 2;
//On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
nb_ordres = 0;
cpt_ordre = 0;
}
}
//Si on n'est pas dans un enchainement...
else if(liste[cpt_ordre].enchainement != 1) {
//On envoie le flag de fin d'instruction correspondant sur CAN
switch(liste[cpt_ordre].type) {
case TYPE_DEPLACEMENT_LIGNE_DROITE :
val1[0] = ASSERVISSEMENT_RECALAGE;
break;
case TYPE_DEPLACEMENT_ROTATION :
val1[0] = ASSERVISSEMENT_ROTATION;
break;
case TYPE_DEPLACEMENT_X_Y_THETA :
val1[0] = ASSERVISSEMENT_XYT;
break;
case TYPE_DEPLACEMENT_RAYON_COURBURE :
val1[0] = ASSERVISSEMENT_COURBURE;
break;
case TYPE_DEPLACEMENT_RECALAGE :
val1[0] = ASSERVISSEMENT_RECALAGE;
break;
default :
val1[0] = 0;
break;
}
//On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
liste[0] = (struct Ordre_deplacement) {
TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
nb_ordres = 0;
cpt_ordre = 0;
cpt = 0;
//On pr�vient qu'on s'est arr�t�
motor_of();
}
}
} else { //On v�rifie le type d'action � effectuer, en fonction du buffer d'instructions
test3++;
switch(liste[cpt_ordre].type) {
case TYPE_DEPLACEMENT_STOP :
Arret();
break;
case TYPE_DEPLACEMENT_IMMOBILE:
test4++;
Asser_Pos_Mot1(roue_drt_init);
Asser_Pos_Mot2(roue_gch_init);
write_PWM2(commande2);
write_PWM1(commande1);
break;
case TYPE_DEPLACEMENT_LIGNE_DROITE:
Avancer_ligne_droite(liste[cpt_ordre].distance, VMAX, AMAX, DMAX);
break;
case TYPE_DEPLACEMENT_ROTATION:
Rotation(liste[cpt_ordre].angle, VMAX, AMAX, DMAX);
break;
case TYPE_DEPLACEMENT_X_Y_THETA:
X_Y_Theta(liste[cpt_ordre].x, liste[cpt_ordre].y, liste[cpt_ordre].theta,
liste[cpt_ordre].sens, VMAX, AMAX);
break;
case TYPE_DEPLACEMENT_RAYON_COURBURE :
Rayon_De_Courbure(liste[cpt_ordre].rayon, liste[cpt_ordre].theta_ray, VMAX,
AMAX, liste[cpt_ordre].sens, DMAX);
break;
case TYPE_DEPLACEMENT_RECALAGE :
Recalage(liste[cpt_ordre].distance, 10, 500, liste[cpt_ordre].recalage, liste[cpt_ordre].val_recalage);
break;
default :
Asser_Pos_Mot1(roue_drt_init);
Asser_Pos_Mot2(roue_gch_init);
write_PWM2(commande2);
write_PWM1(commande1);
test7++;
break;
}
}
//On calcule l'odom�trie � chaque tour de boucle
//MOI*\
ms_count = 0; // reset ms counter
ms_count1 ++;
//Odometrie();
if(ms_count1 == 5)
ms_count1 = 0;
} else {
ms_count = 0; // reset ms counter
ms_count1 ++;
//Odometrie();
if(ms_count1 == 5)
ms_count1 = 0;
}
} else {
ms_count ++;
} /*else if(ms_count % 10 == 0) {
Odometrie();
}
*/
}