/***********************************************************.
|* S U P A E R O * |
|* LABORATOIRE INFORMATIQUE ET AUTOMATIQUE * |
|* * |
|* Commande bras manipulateur 2 axes * |
|* de satellite * |
|* * |
|* * |
|* taches de gestion du bras * |
|* * |
|* Modifications : FIERE Antoinette * |
|* JEAN-THEODORE Olivier * |
|* * |
|* Master Pro CAMSI 2004-2005 * |
*.________________________________________________________.*/
/* Includes */
#include "math.h"
#include "semLib.h"
#include "msgQLib.h"
#include "stdlib.h"
#include "vxWorks.h"
#include "taskLib.h"
#include "stdio.h"
#include "ioLib.h"
#include "intLib.h"
#include "sysLib.h"
#include "calculsBras.h"
#include "socketsTCP386.h"
/*=====================================*/
/* */
/* declarations globales */
/* */
/*=====================================*/
typedef enum {FAUX=0, VRAI=1} booleen;
#define OUVERT VRAI
#define FERME FAUX
typedef struct
{ short int Num_Ordre;
booleen Etat;
} Commande_Pince;
typedef struct
{
float Epaule;
float Coude;
} Coordonnees;
typedef struct
{
short int Num_Ordre;
float Vitesse;
float Epaule;
float Coude;
} Mouvement;
void tPince();
void tEchantillonner();
void tArret();
void Arret_Urgence();
void tCalc_Inter();
void tInit_Calc();
void tSimuler_Bras();
void tAsservissement();
void tDemarrage();
float Conv_Deg_Rad(float);
float Conv_Rad_Deg(float);
/* declaration des identificateurs des taches */
/**********************************************/
int Id_tDialogue;
int Id_tPince;
int Id_tCalc_Inter;
int Id_tSimuler_Bras;
int Id_tAsservissement;
int Id_tEchantillonner;
int Id_tInit_Calc;
int Id_tArret;
/* Identificateurs des semaphores
******************************** */
/* semaphores binaires de synchronisation */
SEM_ID sem_Init_Calcul;
/* periode de l'horloge auxiliaire */
#define FREQUENCE_AUX_CLK 1024
/* variables pour communication et synchronisation entre taches par memoire commune */
float gCoude_Volt;
float gEpaule_Volt;
float gAngle_Epaule_Rad;
float gAngle_Coude_Rad;
int g_T_Echantillon;
booleen gArret_Urg;
booleen gArret;
booleen Connexion_Active;
booleen gEtat_Pince;
/* Declaration des files de messages */
MSG_Q_ID msg_Ordre_Dep_Exec;
MSG_Q_ID msg_Pince_Executee;
MSG_Q_ID msg_Ordre_Pince;
MSG_Q_ID msg_Valeurs_Echantillons;
MSG_Q_ID msg_Ordre_Mvt;
MSG_Q_ID msg_Ordre_Dep_Calc;
MSG_Q_ID msg_Consignes_Moteurs;
/*******************************************************************************
* S/P d'interruption timer()
*
* Description: S/P active tous les 1/1000 seconde
*
*******************************************************************************/
int spitTimer(int entier)
{
static long i = 0;
if(!(i % g_T_Echantillon))
taskResume(Id_tEchantillonner);
if(!(i % 60))
taskResume(Id_tCalc_Inter);
if(!(i % 5))
taskResume(Id_tSimuler_Bras);
if(!(i % 30))
taskResume(Id_tAsservissement);
i++;
}
/************************************************************)
(* *)
(* TACHE tDialogue() *)
(* *)
(************************************************************/
/**************************************/
int tDialogue()
/**************************************/
{
/* declaration des contantes relatives a la tache */
/**************************************************/
#define CARACTERE_COMMANDE_REJOINDRE 'R'
#define CARACTERE_COMMANDE_OUVRIR 'O'
#define CARACTERE_COMMANDE_FERMER 'F'
#define CARACTERE_COMMANDE_ARRET_URGENCE 'A'
#define CARACTERE_LECTURE_CAPTEURS 'L'
#define CARACTERE_LECTURE_ORDRE_EXECUTE 'E'
#define CARACTERE_COMMANDE_EFFECTUEE 'X'
#define CARACTERE_COMMANDE_NON_EFFECTUEE 'N'
#define CARACTERE_COMMANDE_MODIFICATION_PERIODE 'T'
#define CARACTERE_ACQUITTEMENT 'Q'
#define CARACTERE_LECTURE_ETAT_PINCE 'P'
#define CARACTERE_ETAT_PINCE_OUVERTE 'O'
#define CARACTERE_ETAT_PINCE_FERMEE 'F'
#define TAILLE_MAX_TABLEAU_REELS 125
/* declaration des variables locales de la tache */
/**************************************************/
float tableauReels[TAILLE_MAX_TABLEAU_REELS ];
short int nbReelsTableau;
int i;
short int entierLu;
int socketServeur;
int numeroPort=1040;
int erreur; /* indicateur d erreur */
char caractereRecu;
char typeDonnee;
short int retour_depl;
Commande_Pince retour_pince;
Commande_Pince ordre_pince;
int nb_octets;
int nb_octets2;
Coordonnees ech_recu;
Mouvement Mouv;
printf("dialogue :1\n");
while(1)
{
erreur = 0;
Connexion_Active=FAUX;
/* ouverture socket serveur */
socketServeur= ouvrirSocketServer(numeroPort);
printf(" port %d ouvert\n",numeroPort);
if (socketServeur == ERROR)
{
printf("Erreur ouverture socket\n");
erreur=1;
}
Connexion_Active=VRAI;
/* boucle de lecture des requetes */
while (erreur==0)
{
typeDonnee=lectureMessage();
if (typeDonnee< 0) /* erreur de lecture socket */
{
printf(" erreur de lecture socket");
erreur=1;
}
else
{
switch (typeDonnee)
{
case TYPE_CAR : /* requetes de type caractere */
{
caractereRecu= retourneCaractereLu();
switch (caractereRecu)
{
case CARACTERE_LECTURE_ORDRE_EXECUTE : /* lecture ordre execute */
{
nb_octets = msgQReceive(msg_Ordre_Dep_Exec, (char *)&retour_depl, sizeof(short int), NO_WAIT);
if(nb_octets > 0)
ecrireEntier(CARACTERE_COMMANDE_EFFECTUEE, retour_depl);
nb_octets2 = msgQReceive(msg_Pince_Executee, (char *)&retour_pince, sizeof(Commande_Pince), NO_WAIT);
if(nb_octets2 > 0)
ecrireEntier(CARACTERE_COMMANDE_EFFECTUEE, retour_pince.Num_Ordre);
else
{
if(nb_octets <= 0)
ecrireEntier(CARACTERE_COMMANDE_NON_EFFECTUEE,0);
}
}
break;
case CARACTERE_COMMANDE_ARRET_URGENCE : /* commande arret urgence */
{
Arret_Urgence();
ecrireCaractere(CARACTERE_ACQUITTEMENT );
}
break;
case CARACTERE_LECTURE_CAPTEURS : /* Lecture bloc des derniers echantillons */
{
/******* completer ******/
/* renvoyer tableau de reels contenant les positions angulaires en degres*/
/* par defaut : tableau de 10 echantillons */
for (i=0; i<10; i++)
{
tableauReels[i*2]= 0;
tableauReels[(i*2)+1]= 0;
}
i = 0;
while(msgQReceive(msg_Valeurs_Echantillons, (char *)&ech_recu, sizeof(Coordonnees), NO_WAIT) > 0)
{
tableauReels[2*i] = ech_recu.Epaule;
tableauReels[2*i +1] = ech_recu.Coude;
i++;
}
ecrireTableauReels(CARACTERE_ACQUITTEMENT ,tableauReels,20);
}
break;
case CARACTERE_LECTURE_ETAT_PINCE : /* Lecture bloc des derniers echantillons */
{
if(gEtat_Pince == OUVERT)
ecrireCaractere(CARACTERE_ETAT_PINCE_OUVERTE);
else
ecrireCaractere(CARACTERE_ETAT_PINCE_FERMEE);
}
}
}
break;
case TYPE_ENTIER : /* requetes de type entier */
{
caractereRecu= retourneCaractereLu();
entierLu=retourneEntierLu();
switch (caractereRecu)
{
case CARACTERE_COMMANDE_OUVRIR : /* commande ouvrir */
{
/* prise en compte de la commande : entierLu contient le num‚ro de l'ordre */
gArret_Urg = FAUX;
ordre_pince.Num_Ordre = entierLu;
ordre_pince.Etat = OUVERT;
if(msgQSend(msg_Ordre_Pince, (char *)&ordre_pince, sizeof(Commande_Pince), NO_WAIT, MSG_PRI_NORMAL) == OK)
ecrireCaractere(CARACTERE_ACQUITTEMENT);
}
break;
case CARACTERE_COMMANDE_FERMER : /* commande fermer */
{
/* prise en compte de la commande : entierLu contient le numero de l'ordre */
gArret_Urg = FAUX;
ordre_pince.Num_Ordre = entierLu;
ordre_pince.Etat = FERME;
if(msgQSend(msg_Ordre_Pince, (char *)&ordre_pince, sizeof(Commande_Pince), NO_WAIT, MSG_PRI_NORMAL) == OK)
ecrireCaractere(CARACTERE_ACQUITTEMENT);
}
break;
case CARACTERE_COMMANDE_MODIFICATION_PERIODE : /* commande modification periode */
{
/* prise en compte de la commande : entierLu contient le num‚ro de l'ordre */
g_T_Echantillon = entierLu;
ecrireCaractere(CARACTERE_ACQUITTEMENT );
}
break;
}
}
case TYPE_TABLEAU_REELS : /* requetes de type tableau reels */
{
caractereRecu= retourneCaractereLu();
retourneTableauReelsLu(tableauReels,&nbReelsTableau);
switch (caractereRecu)
{
case CARACTERE_COMMANDE_REJOINDRE: /* commande rejoindre */
{
/* prise en compte de la commande : TableauReels contient les parametres de l'ordre */
gArret_Urg = FAUX;
Mouv.Num_Ordre = tableauReels[0];
Mouv.Vitesse = tableauReels[1];
Mouv.Epaule = Conv_Deg_Rad(tableauReels[2]);
Mouv.Coude = Conv_Deg_Rad(tableauReels[3]);
if(msgQSend(msg_Ordre_Mvt, (char *)&Mouv, sizeof(Mouvement), NO_WAIT, MSG_PRI_NORMAL) == OK)
ecrireCaractere(CARACTERE_ACQUITTEMENT);
}
break;
}
}
} /* fin du switch */
} /* fin du else erreur */
} /* fin du while erreur==0 */
fermetureSocket();
printf(" port %d ferme",numeroPort);
}
}
/*******************************************************************************
*
* Tache tDemarrage()
*
* Description: cree tous les elements composant l'application
*
*******************************************************************************/
void tDemarrage()
{
/*Initialisation de l'horloge auxiliaire*/
/* premiere operation a realiser */
sysAuxClkRateSet(FREQUENCE_AUX_CLK); /*1000 ticks/secondes=> 1 ms*/
sysAuxClkConnect(spitTimer, 0 ); /*fonction directement connectee*/
/* initialisation des variables globales */
/*****************************************/
Connexion_Active=FAUX;
gCoude_Volt = 0;
gEpaule_Volt = 0;
gAngle_Epaule_Rad = 0;
gAngle_Coude_Rad = 0;
g_T_Echantillon = 10;
gArret = FAUX;
gArret_Urg = FAUX;
/* creation des files de messages */
msg_Valeurs_Echantillons = msgQCreate(100, 2*sizeof(float), MSG_Q_FIFO);
msg_Ordre_Pince = msgQCreate(10, sizeof(Commande_Pince), MSG_Q_FIFO);
msg_Pince_Executee = msgQCreate(10, sizeof(Commande_Pince), MSG_Q_FIFO);
msg_Ordre_Dep_Exec = msgQCreate(10, sizeof(int), MSG_Q_FIFO);
msg_Ordre_Mvt = msgQCreate(10, sizeof(int) + 3*sizeof(float), MSG_Q_FIFO);
msg_Ordre_Dep_Calc = msgQCreate(1, sizeof(int), MSG_Q_FIFO);
msg_Consignes_Moteurs = msgQCreate(1, sizeof(Coordonnees), MSG_Q_FIFO);
/* creation des semaphores binaires */
sem_Init_Calcul = semBCreate(SEM_Q_FIFO, SEM_FULL);
/* creation des taches */
Id_tDialogue = taskSpawn ("tDialogue",100, VX_FP_TASK, 10000, (FUNCPTR)tDialogue, 0,0,0,0,0,0,0,0,0,0);
Id_tAsservissement = taskSpawn ("tAsservissement", 200, VX_FP_TASK, 10000, (FUNCPTR)tAsservissement , 0,0,0,0,0,0,0,0,0,0);
Id_tInit_Calc = taskSpawn ("tInit_Calc",200, VX_FP_TASK, 10000, (FUNCPTR)tInit_Calc , 0,0,0,0,0,0,0,0,0,0);
Id_tCalc_Inter = taskSpawn ("tCalc_Inter",200, VX_FP_TASK, 10000, (FUNCPTR)tCalc_Inter , 0,0,0,0,0,0,0,0,0,0);
Id_tSimuler_Bras = taskSpawn ("tSimuler_Bras",150, VX_FP_TASK, 10000, (FUNCPTR)tSimuler_Bras , 0,0,0,0,0,0,0,0,0,0);
Id_tEchantillonner = taskSpawn ("tEchantillonner",150, VX_FP_TASK, 10000, (FUNCPTR)tEchantillonner , 0,0,0,0,0,0,0,0,0,0);
Id_tPince = taskSpawn ("tPince", 250, VX_FP_TASK, 10000, (FUNCPTR)tPince , 0,0,0,0,0,0,0,0,0,0);
sysAuxClkEnable(); /*Activation du Timer*/
exit(1);
}
/************************************************************)
(* *)
(* TACHE tInitCalc() *)
(* *)
(************************************************************/
void tInit_Calc()
{
Mouvement Ordre_Mouvement;
short i;
while(1)
{
msgQReceive( msg_Ordre_Mvt, (char *) &Ordre_Mouvement, sizeof(Mouvement),WAIT_FOREVER);
semTake(sem_Init_Calcul, WAIT_FOREVER);
if(gArret_Urg == FAUX)
{
initCalculsProfils ( gAngle_Epaule_Rad, gAngle_Coude_Rad, Ordre_Mouvement.Vitesse ,Ordre_Mouvement.Epaule, Ordre_Mouvement.Coude);
i = Ordre_Mouvement.Num_Ordre;
msgQSend( msg_Ordre_Dep_Calc, (char *) &i, sizeof(short int), NO_WAIT, MSG_PRI_NORMAL);
}
else
semGive(sem_Init_Calcul);
}
} /* fin tInit_Calc */
/************************************************************)
(* *)
(* TACHE tCalcInter() *)
(* *)
(************************************************************/
void tCalc_Inter()
{
short lNum_Ordre;
Coordonnees Consigne;
float *positionIntermediaireEpaule = (float *)malloc(sizeof(float));
float *positionIntermediaireCoude = (float *)malloc(sizeof(float));
short *finMouvement = (short *)malloc(sizeof(short));
while(1)
{
*finMouvement = FAUX;
msgQReceive( msg_Ordre_Dep_Calc, (char *)&lNum_Ordre, sizeof(short int), WAIT_FOREVER);
do
{
calculPositionIntermediaire(positionIntermediaireEpaule, positionIntermediaireCoude, finMouvement);
Consigne.Epaule = *positionIntermediaireEpaule;
Consigne.Coude = *positionIntermediaireCoude;
msgQSend( msg_Consignes_Moteurs, (char *)&Consigne, sizeof(Coordonnees), NO_WAIT, MSG_PRI_NORMAL);
taskSuspend(taskIdSelf());
}
while(gArret_Urg == FAUX && *finMouvement == FAUX);
if (*finMouvement == VRAI)
msgQSend( msg_Ordre_Dep_Exec, (char *)&lNum_Ordre, sizeof(short), NO_WAIT, MSG_PRI_NORMAL);
printf("Epaule : %f, Coude : %f\n", gAngle_Epaule_Rad, gAngle_Coude_Rad);
semGive(sem_Init_Calcul);
}
} /* fin tCalc_Inter */
/************************************************************)
(* *)
(* TACHE tAsservissement() *)
(* *)
(************************************************************/
void tAsservissement()
{
int nb_octets;
Coordonnees consigne;
while(1)
{
taskSuspend(taskIdSelf());
nb_octets = msgQReceive(msg_Consignes_Moteurs, (char *)&consigne, sizeof(Coordonnees), NO_WAIT);
if(gArret == FAUX && gArret_Urg == FAUX && nb_octets > 0)
{
gEpaule_Volt = asservissementMoteurEpaule(gAngle_Epaule_Rad, consigne.Epaule);
gCoude_Volt = asservissementMoteurCoude(gAngle_Coude_Rad, consigne.Coude);
}
else
{
gEpaule_Volt = 0;
gCoude_Volt = 0;
}
}
}
/************************************************************)
(* *)
(* TACHE tEchantillonner() *)
(* *)
(************************************************************/
void tEchantillonner()
{
Coordonnees E;
while(1)
{
taskSuspend(taskIdSelf());
if(Connexion_Active == VRAI)
{
E.Epaule = Conv_Rad_Deg(gAngle_Epaule_Rad);
E.Coude = Conv_Rad_Deg(gAngle_Coude_Rad);
msgQSend(msg_Valeurs_Echantillons, (char *)&E, sizeof(E), WAIT_FOREVER, MSG_PRI_NORMAL);
}
}
}
/************************************************************)
(* *)
(* TACHE tPince() *)
(* *)
(************************************************************/
void tPince()
{
Commande_Pince *Commande_Courante = (Commande_Pince *)malloc(sizeof(Commande_Pince));
gEtat_Pince = FERME;
while(1)
{
msgQReceive(msg_Ordre_Pince, (char *)Commande_Courante, sizeof(Commande_Pince), WAIT_FOREVER);
taskDelay(sysClkRateGet());
gEtat_Pince = Commande_Courante->Etat;
msgQSend(msg_Pince_Executee, (char *)Commande_Courante, sizeof(Commande_Pince), WAIT_FOREVER, MSG_PRI_NORMAL);
}
}
/************************************************************)
(* *)
(* TACHE tSimuler_Bras() *)
(* *)
(************************************************************/
void tSimuler_Bras()
{
initSimulationEpaule(gAngle_Epaule_Rad);
i nitSimulationCoude(gAngle_Coude_Rad);
while(1)
{
gAngle_Epaule_Rad = simulationEpaule(gEpaule_Volt);
gAngle_Coude_Rad = simulationCoude(gCoude_Volt);
taskSuspend(taskIdSelf());
}
}
/*******************************************************************************
*
* Tache tArret()
*
* Description: Detruit tous les elements composant l'application
*
*******************************************************************************/
void tArret( )
{
/*Arret de l'horloge auxiliaire*/
sysAuxClkDisable(); /* Desactivation du Timer */
printf("Arret de l'ensemble des taches de l'application\n");
/* effacement des taches */
taskSuspend(Id_tDialogue);
fermetureSocket();
taskDelete (Id_tDialogue);
taskSuspend(Id_tAsservissement);
taskDelete(Id_tAsservissement);
taskSuspend(Id_tCalc_Inter);
taskDelete(Id_tCalc_Inter);
taskSuspend(Id_tInit_Calc);
taskDelete(Id_tInit_Calc);
taskSuspend(Id_tSimuler_Bras);
taskDelete(Id_tSimuler_Bras);
taskSuspend(Id_tEchantillonner);
taskDelete(Id_tEchantillonner);
taskSuspend(Id_tPince);
taskDelete(Id_tPince);
/* effacement des files */
msgQDelete(msg_Pince_Executee);
msgQDelete(msg_Ordre_Pince);
msgQDelete(msg_Valeurs_Echantillons);
msgQDelete(msg_Ordre_Mvt);
msgQDelete(msg_Ordre_Dep_Calc);
msgQDelete(msg_Consignes_Moteurs);
/* effacement des semaphores */
semDelete(sem_Init_Calcul);
printf("Fin de l'application\n");
exit(1);
}
/* Fonctions auxiliaires */
/********************************************
* Fonction arret d'urgence
* Vide la file d'ordres de mouvements
* et signale aux taches concernees
* qu'un arret d'urgence est en cours
*********************************************/
void Arret_Urgence()
{
Mouvement *Ordre_Mouvement = (Mouvement *)malloc(sizeof(Mouvement));
gArret_Urg = VRAI;
while (msgQNumMsgs(msg_Ordre_Mvt) > 0)
msgQReceive( msg_Ordre_Mvt, (char*) Ordre_Mouvement, sizeof(Mouvement),NO_WAIT);
printf("Arret d'urgence effectue\n");
}
float Conv_Rad_Deg(float x)
{
return (180 * x / PI);
}
float Conv_Deg_Rad(float x)
{
return (PI * x / 180);
}