Projet

Général

Profil

« Précédent | Suivant » 

Révision 372

Ajouté par maporte3 il y a plus de 6 ans

Gestion des pistes : la vitesse dépend de la piste en cours

Voir les différences:

branch/porte/Emb_App/SessionM32C_E8a_system.ini
[Target]
M32C E8a SYSTEM=Renesas Communications
[USER_DATA]
RESET=ff0068
RESET=ff0076
branch/porte/Emb_App/programme_principal_etud.c
//'j'/106/06A : R?cup?ration du r?sultat de dernier code envoy?. 0x77 si aucun code n'a ?t? soumis. <0 si la r?ponse n'est pas
// disponible. 0xab avec a-> nombre de couleurs bien plac?es et b -> couleurs pr?sentes mais mal plac?es.
//'I'/73/Ox49 : D?finition du nom du v?hicule. Doit d?buter par le caract?re '#' et entraine le chargement de la configuration de piste
// correspondant au nom du v?hicule si le nom se termine par '*'
// correspondant au nom du v?hicule si le nom se termine par '*'
// +-----------------------------------------------------+
// | Constantes |
// +-----------------------------------------------------+
const Vitesse_max=99, Vitesse_min=0, Vitesse_noir=12;
const Kpt=1 , Kdt=0.1, Kdr=0.2, Kpr=1; //Param?tre des PD de la tourelle et des roues
/*-------------------------------------- Constante ----------------------------------------*/
const Vitesse_max=99, Vitesse_min=0;
/*-------------------------------------- Variables ----------------------------------------*/
// +-----------------------------------------------------+
// | Variables |
// +-----------------------------------------------------+
//Variables pour l'asserivssement des roues, de la vitesse et du telemetre
unsigned int Angle_T=0, Consigne_T=500; //prend la valeur de l'angle au temps t-1 //Consigne de l'angle de la tourelle (consigne/10=>degr?s)
unsigned int Angle_T=0, Consigne_T=400, Consigne_T_noir=900; //prend la valeur de l'angle au temps t-1 //Consigne de l'angle de la tourelle (consigne/10=>degr?s)
unsigned int valeur=0, Saut=0; //valeur transmise corrig?
unsigned int Somme_Erreur=0, Erreur=0;
unsigned int ErreurRoues=0,Somme_ErreurRoues=0;
int Kpt=1, Kpr=1; //Param?tre des PID tourelle et roues
int Kdt=0.1, Kdr=0.1;
unsigned int Vitesse=40, vit=0, i=1, Distance;
unsigned int Angle_R=0, distance_bord=660; //Angle des roues
unsigned int vit=0, i=1, Distance;
unsigned int Angle_R=0, Vitesse_vert=40,Vitesse_bleu=35,Vitesse_rouge=30; //Angle des roues
//Variable de la fonction Capteur
unsigned int Info_capteur;
unsigned int Info_capteur, couleur_capteur, num_capteur;
//Variable de la fonction Distance_bord
unsigned int Bord_defaut=0, ecart=400;
unsigned int Bord_defaut=0, ecart=400, distance_bord=790, distance_bord_noir=500;
//Variable des fonctions IHM
short Touche_clavier, Temps;
short Touche_clavier, Temps=0;
//char *Temps;
//Variable de la fonction Circuit
unsigned int Info_circuit, Etat_feu, Num_circuit, Depart_ok=0, Arret_urgence=0;
unsigned int Info_circuit, Etat_feu, Num_circuit=0, Depart_ok=0, Arret_urgence=0;
//Variable de la fonction Evenement
unsigned int Lecture_eve;
FLGPTN test;
// +-----------------------------------------------------+
// | Asservissement tourelle |
// +-----------------------------------------------------+
void Asserv_T(){
//'T'/84/0x54?: Commande en vitesse de la tourelle portant le telemetre (en 1/10 de degre/secondes).
......
demande.data.id='R';
demande.data.rtr=1;
snd_dtq (CanTx,demande.msg); // Interrogation du peripherique
Angle_T=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
snd_dtq (CanTx,demande.msg); // Interrogation du peripherique sur la position de la tourelle
Angle_T=periph[ADDR('R')].val; // contient la valeur de retour du simulateur de l'angle de la tourelle.
comm.data.id='T';
comm.data.rtr=0;
comm.data.val=valeur;
snd_dtq (CanTx,comm.msg);//Envoi de la commande
comm.data.val=valeur; // Commande de l'angle de la tourelle
snd_dtq (CanTx,comm.msg); // Envoi de la commande
dly_tsk(100);
}
......
void Asserv_T_hc(){
Erreur=Consigne_T-Angle_T;
Somme_Erreur+=Erreur;
switch(Num_circuit){
case 1:
Erreur=Consigne_T-Angle_T;
break;
case 2:
Erreur=Consigne_T-Angle_T;
break;
case 3:
Erreur=Consigne_T-Angle_T;
break;
case 4:
Erreur=Consigne_T_noir-Angle_T;
break;
}
if(Angle_T!=Consigne_T)
valeur=Kpt*Erreur+Kdt*Somme_Erreur;//correction de l'angle par handler cyclique de la tourelle
valeur=Kpt*Erreur+Kdt*Somme_Erreur; //correction de l'angle par handler cyclique de la tourelle
}
// +-----------------------------------------------------+
// | Asservissement Roues |
// +-----------------------------------------------------+
void Asserv_R(){
//'D'/68/0x44?: Commande de l'angle des roues directrices (en 1/10 de degre).
......
CanFrame demande;
CanFrame reponse;
dly_tsk(1500);
dly_tsk(1000);
while(1){
comm.data.id='D'; //Commande de l'angle des roues
......
ErreurRoues=distance_bord-Distance;
Somme_ErreurRoues+=ErreurRoues;
if(Distance != distance_bord && Bord_defaut == 0)
Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);//correction de l'angle par handler cyclique des roues
switch(Num_circuit){
case 1:
if(Distance != distance_bord && Bord_defaut == 0){
Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);} //correction de l'angle par handler cyclique des roues
break;
case 2:
if(Distance != distance_bord && Bord_defaut == 0){
Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);} //correction de l'angle par handler cyclique des roues
break;
case 3:
if(Distance != distance_bord && Bord_defaut == 0 && num_capteur != 0x66F0){
Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);} //correction de l'angle par handler cyclique des roues
break;
case 4:
if(Distance != distance_bord_noir && Bord_defaut == 0){
Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);} //correction de l'angle par handler cyclique des roues
break;
default :
if(Distance != distance_bord && Bord_defaut == 0){
Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);}
}
}
// +-----------------------------------------------------+
// | Asservissement Vitesse |
// +-----------------------------------------------------+
void Asserv_V(){
//'V'/86/0x56?: Commande en vitesse des roues motrices du vehicule (en radian /secondes).
......
CanFrame demande;
CanFrame reponse;
dly_tsk(3000);
dly_tsk(2500);
while(1){
comm.data.id='V'; //Commande de la vitesse des roues motrices
comm.data.rtr=0;
if(Depart_ok == 1 && Arret_urgence == 0){
if(Info_capteur == 0x560){ // Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
comm.data.val=Vitesse;
snd_dtq (CanTx,comm.msg); // Envoi de la commande de vitesse
}
else if(Info_capteur == 0x630 && Saut == 0){ //Accel?ration pour passer le saut
comm.data.val=Vitesse*1.66;
switch(Num_circuit){
case 1:
if(Depart_ok == 1 && Arret_urgence == 0){
if(couleur_capteur == 0x560){ // Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
comm.data.val=Vitesse_vert;
snd_dtq (CanTx,comm.msg); // Envoi de la commande de vitesse
}
else{ // Ralentissement en cas de virage.
comm.data.val=Vitesse_vert/1.2;
snd_dtq (CanTx,comm.msg);
}
}
else{
comm.data.val=0;
snd_dtq (CanTx,comm.msg);
}
break;
case 2:
if(Depart_ok == 1 && Arret_urgence == 0){
if(couleur_capteur == 0x560){ // Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
comm.data.val=Vitesse_bleu;
snd_dtq (CanTx,comm.msg); // Envoi de la commande de vitesse
}
else{ // Ralentissement en cas de virage.
comm.data.val=Vitesse_bleu/1.3;
snd_dtq (CanTx,comm.msg);
}
}
else{
comm.data.val=0;
snd_dtq (CanTx,comm.msg);
}
break;
case 3 :
if(Depart_ok == 1 && Arret_urgence == 0){
if(couleur_capteur == 0x560){ // Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
comm.data.val=Vitesse_rouge;
snd_dtq (CanTx,comm.msg); // Envoi de la commande de vitesse
}
else if(couleur_capteur == 0x0766 && num_capteur == 0x66F0){ //Accel?ration pour passer le saut
comm.data.val=45;
snd_dtq (CanTx,comm.msg);
}
else{ // Ralentissement en cas de virage.
comm.data.val=Vitesse_rouge/1.5;
snd_dtq (CanTx,comm.msg);
}
}
break;
case 4 :
if(Depart_ok == 1 && Arret_urgence == 0){
comm.data.val=Vitesse_noir;
snd_dtq (CanTx,comm.msg);
}
break;
default :
comm.data.val=0;
snd_dtq (CanTx,comm.msg);
Saut=1;
}
else if(Info_capteur == 0x630 && Saut == 1){ //Ralentissement pour la reception du saut
comm.data.val=Vitesse/2.3;
snd_dtq (CanTx,comm.msg);
// Saut=0;
}
else{ // Ralentissement en cas de virage et de d?tection de trou ou d'obstacle.
comm.data.val=Vitesse/1.55;
snd_dtq (CanTx,comm.msg);
}
}
else{
comm.data.val=0;
snd_dtq (CanTx,comm.msg);
}
dly_tsk(10);
}
}
// +-----------------------------------------------------+
// | Commande capteur |
// +-----------------------------------------------------+
void Capteur(){
//'C'/67/0x43?: Informations sur le dernier capteur touche :
......
demande.data.id='C';
demande.data.rtr=1;
snd_dtq (CanTx,demande.msg); // Interrogation du peripherique sur les donnees du dernier capteur touch?.
Info_capteur=periph[ADDR('C')].val>>4; // contient la valeur de retour du simulateur sur le dernier capteur touch?.
snd_dtq (CanTx,demande.msg); // Interrogation du peripherique sur les donnees du dernier capteur touch?.
Info_capteur=periph[ADDR('C')].val; // contient la valeur de retour du simulateur sur le dernier capteur touch?.
couleur_capteur=Info_capteur >> 4;
num_capteur=Info_capteur << 4;
dly_tsk(100);
}
......
}
// +-----------------------------------------------------+
// | Gestion des troues |
// +-----------------------------------------------------+
void Distance_bord(){
//'U'/85/0x55?: Distance mesuree par le telemetre (1/100 de metre).
......
snd_dtq (CanTx,demande.msg); // Interrogation du peripherique sur la distance mesure par le telemetre.
Distance=periph[ADDR('U')].val; // contient la valeur de retour du simulateur sur la distance mesur? par le telemetre.
if(Distance > distance_bord+ecart || Distance < distance_bord-ecart){
Bord_defaut=1;
switch(Num_circuit){
case 1:
if(Distance > distance_bord+ecart || Distance < distance_bord-ecart){
Bord_defaut=1;
}
else{
Bord_defaut=0;
}
break;
case 2:
if(Distance > distance_bord+ecart || Distance < distance_bord-ecart){
Bord_defaut=1;
}
else{
Bord_defaut=0;
}
break;
case 3:
if(Distance > distance_bord+ecart || Distance < distance_bord-ecart){
Bord_defaut=1;
}
else{
Bord_defaut=0;
}
break;
case 4:
if(Distance > distance_bord_noir+ecart || Distance < distance_bord_noir-ecart){
Bord_defaut=1;
}
else{
Bord_defaut=0;
}
break;
}
else{
Bord_defaut=0;
}
dly_tsk(10);
dly_tsk(10);
}
}
// +-----------------------------------------------------+
// | Commande IHM |
// +-----------------------------------------------------+
void Commande_clavier(){
// Interface Homme Machine muni du clavier matricielle, de l'?cran LCD,
......
}
else if(Touche_clavier == 42 || Touche_clavier == 35){
if(vit < 100 && vit > 0){
Vitesse = vit;
Vitesse_vert = vit;
}
vit = 0;
i = 1;
......
}
// +-----------------------------------------------------+
// | Gestion des informations de la piste |
// +-----------------------------------------------------+
void Circuit(){
//'M'/77/0x7D?: Mode de course :
......
}
}
// +-----------------------------------------------------+
// | main |
// +-----------------------------------------------------+
void main()
{
ports_mcu();

Formats disponibles : Unified diff