|
/***********************************************************************/
|
|
/* */
|
|
/* FILE :test_compil.c */
|
|
/* DATE :Fri, Sep 29, 2006 */
|
|
/* DESCRIPTION :main program file. */
|
|
/* CPU GROUP :87 */
|
|
/* */
|
|
/* This file is generated by Renesas Project Generator (Ver.4.5). */
|
|
/* m308 */
|
|
/* nc308lib */
|
|
/* c308mr */
|
|
/* nc382lib */
|
|
/* */
|
|
/***********************************************************************/
|
|
#include "sfr32c87.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <itron.h>
|
|
#include <kernel.h>
|
|
#include "kernel_id.h"
|
|
#include "lcd.h"
|
|
#include "clavier.h"
|
|
#include "periph.h"
|
|
#include "uart0.h"
|
|
#include "can.h"
|
|
#include "carte_io.h"
|
|
#include "carte_m32.h"
|
|
#include <math.h>
|
|
|
|
// Potentiometre: lire les registres ad00 et ad01, les valeurs sont sur 10 bits.
|
|
|
|
// Clavier: vrcv_dtq(QdmTouche,&code_touche) pour lire la derniere touche appuyee sur le clavier.
|
|
// la variable code_touche doit etre du type short.
|
|
|
|
// Bouton poussoir: Bp_G, Bp_M, Bp_D permettent de lire l'etat des boutons de la carte I/O
|
|
|
|
// Leds: LED_R=1 ou LED_R=0 Pour allumer ou eteindre les leds (LED_R, LED_J, LED_V).
|
|
|
|
// Pour communiquer avec le simulateur utiliser une variable de type CanFrame,
|
|
// Definir les differents champs en utilisant la structure (S)eparee (comm.data)
|
|
// Envoyer le message complet en utilisant l'union (comm.msg)
|
|
|
|
// Exemple:
|
|
// CanFrame comm;
|
|
// comm.data.id='T'; comm.data.rtr=0; comm.data.val=-100;
|
|
// snd_dtq (CanTx,comm.msg);
|
|
|
|
// Pour interroger un peripherique et recuperer les donnees brutes renvoyees simulateur:
|
|
// CanFrame demande;
|
|
// CanFrame reponse;
|
|
//
|
|
// demande.data.id='R'; demande.data.rtr=1;
|
|
// snd_dtq (CanTx,demande.msg); // Interrogation du peripherique
|
|
// rcv_dtq (CanRx,&reponse.msg); // Attente de la reponse
|
|
// reponse.data.val contient la valeur de retour du simulateur.
|
|
// ATTENTION: Ne pas utiliser rcv_dtq(CanRx... si la tache ID_periph_rx est active
|
|
|
|
// Lors de l'utilisation de la tache de reception et distribution des messages ID_periph_rx
|
|
// Demarrer cette tache : sta_tsk(ID_periph_rx);
|
|
// Pour lire la valeur d'un peripherique:
|
|
// Il faut envoyer une demande de lecture:
|
|
// CanFrame comm;
|
|
// comm.data.id='R'; comm.data.rtr=1;
|
|
// snd_dtq (CanTx,comm.msg);
|
|
//
|
|
// Des l'arrivee de la reponse du simulateur, les variables suivantes sont mises a jour:
|
|
// periph[ADDR('R')].val : contient la derniere valeur renvoyee par le simulateur.
|
|
//
|
|
// Pour verifier si une nouvelle valeur a ete recue utiliser:
|
|
// periph[ADDR('R')].maj (incremente a chaque reception).
|
|
|
|
// Pour qu'un evenement soit declenche lors de la reception d'une donnee pour un peripherique:
|
|
// periph[ADDR('R')].ev=0x01;
|
|
// Pour se mettre en attente de l'evenement: wai_flg (ev_periph,0x01,TWF_ORW,&flag); // Declarer la variable flag comme : FLGPTN flag
|
|
// Attention l'evenement n'est pas efface apres reception, il faut donc utiliser clr_flg(ev_periph,~0x01); par example
|
|
|
|
// Les evenements:
|
|
// Si le simulateur envoi un evenement sur 16 bits il est recu grace a:
|
|
// par exemple: wai_flg(event,(FLGPTN) 0x0007,TWF_ORW,&flag); // Declarer la variable flag comme : FLGPTN flag
|
|
// Attention l'evenement n'est pas efface apres reception, il faut donc utiliser clr_flg(event,~flag); par example
|
|
//Bit Information associee Remarque
|
|
//0 Capteur Vert, remis a zero lors de la lecture du peripherique 'C'
|
|
//1 Capteur Jaune, remis a zero lors de la lecture du peripherique 'C'
|
|
//2 Capteur Rouge, remis a zero lors de la lecture du peripherique 'C'
|
|
//3 Capteur Bleu, remis a zero lors de la lecture du peripherique 'C'
|
|
//4 Capteur Cyan, remis a zero lors de la lecture du peripherique 'C'
|
|
//5
|
|
//6 Collision avec le sol, remise a zero au changement de piste.
|
|
//7 Fin de course (capteur vert), remis a zero lors de la lecture du peripherique 'C'
|
|
//8 La piste a change , remis a zero lors de la lecture du peripherique 'M'
|
|
//9 Le mode de course a change , remis a zero lors de la lecture du peripherique 'M'
|
|
//10
|
|
//11 Le vehicule a termine un tour, remis a zero au changement du mode de course.
|
|
//12 Sortie de la piste,
|
|
//13 Teleportation a ete utilisee, remis a zero au changement de piste ou du mode de course.
|
|
//14 Faux depart remise a zero au changement du mode de course.
|
|
//15
|
|
|
|
|
|
// Peripheriques disponibles:
|
|
//'V'/86/0x56 : Commande en vitesse des roues motrices du vehicule (en radian /secondes).
|
|
//'D'/68/0x44 : Commande de l'angle des roues directrices (en 1/10 de degre).
|
|
//'T'/84/0x54 : Commande en vitesse de la tourelle portant le telemetre (en 1/10 de degre/secondes).
|
|
//'R'/82/0x52 : Lecture de l'angle effectif de la tourelle portant le telemetre (en 1/10 de degre).
|
|
//'U'/85/0x55 : Distance mesuree par le telemetre (1/100 de metre)
|
|
//'N'/78/0x4E : Numero de la voiture (en fonction de l'ordre de connexion)
|
|
//'E'/69/0x45 : Lecture des evenements,
|
|
//'H'/72/0x48 : Donne le temps de course actuel
|
|
//'S'/83/0x53 : Temps du tour precedent
|
|
//'M'/77/0x7D : Mode de course :
|
|
// Bit 15 : Etat feu tricolore ( 1 -> Vert, 0 -> Orange ou Rouge),
|
|
// Bits 14-8 : 1 Attente, 2 course, 3 essais libres)
|
|
// Bits 7-0 : numero de la piste
|
|
//'C'/67/0x43 : Informations sur le dernier capteur touche :
|
|
// 8 bits de poids faible : numero du capteur
|
|
// 8 bits de poids fort : couleur ('C','R','J','B' ou 'V')
|
|
//'J'/74/0x4A : Proposition d'un code de deverouillage.
|
|
//'j'/106/06A : Recuperation du resultat de dernier code envoye. 0x77 si aucun code n'a ete soumis. <0 si la reponse n'est pas
|
|
// disponible. 0xab avec a-> nombre de couleurs bien plac?es et b -> couleurs presentes mais mal placees.
|
|
//'I'/73/Ox49 : Definition du nom du vehicule. Doit d?buter par le caractere '#' et entraine le chargement de la configuration de piste
|
|
// correspondant au nom du vehicule si le nom se termine par '*'
|
|
|
|
// Definition des codes commandes et requetes
|
|
#define VITESSE_ROUES 'V'
|
|
#define ANGLE_ROUES 'D'
|
|
#define VITESSE_TOURELLE 'T'
|
|
#define ANGLE_TOURELLE 'R'
|
|
#define DISTANCE_TELEMETRE 'U'
|
|
#define MODE_COURSE 'M'
|
|
#define INFO_CAPTEUR 'C'
|
|
|
|
//Definition des couleurs de piste
|
|
#define PISTE_VERTE 1
|
|
#define PISTE_BLEUE 2
|
|
#define PISTE_ROUGE 3
|
|
#define PISTE_NOIRE 4
|
|
|
|
void strategie(VP_INT stacd);
|
|
void couleur_piste(void);
|
|
void rotation_tourelle(unsigned int angle);
|
|
int position_tourelle(void);
|
|
void asserv_tourelle(VP_INT stacd);
|
|
short distance_telemetre(void);
|
|
void vitesse_roue(unsigned int vitesse);
|
|
void angle_roue(unsigned int angle);
|
|
void asserv_roue(VP_INT stacd);
|
|
unsigned int send_requete(unsigned char id_periph);
|
|
void send_commande(unsigned char id_periph, unsigned int val);
|
|
|
|
|
|
short distance_mur = 0;
|
|
short angle_tourelle = 450;
|
|
|
|
short vit_max = 30;
|
|
short vit_max_verte = 70;
|
|
short vit_virage_verte = 32;
|
|
short vit_max_rouge = 35;
|
|
unsigned int vit_roue = 25;//*
|
|
char last_vitesse;
|
|
short K_roue = 1;
|
|
short ang_roue = 0;
|
|
short capteur = 0;
|
|
|
|
short compar_mur = 500;
|
|
|
|
char num_piste = -1;
|
|
char nbre_tours = 1;
|
|
|
|
char rotation_roues_asserv = 1; //Si 1 roues libres sinon blocage
|
|
char rotation_roues_strat = 1; //Si 1 roues libres sinon blocage
|
|
short filtre_sup = 1000;
|
|
short filtre_inf = 0;
|
|
|
|
char pseudo_sem = 0;
|
|
char BR_appuye = 0; //1 Si le bouton rouge ete appuye, 0 sinon
|
|
|
|
short code_secret = 0123;
|
|
|
|
unsigned char charge_cpu = 0; //en %
|
|
unsigned long cpt_cpu = 0;
|
|
unsigned long cpt_cpu_max = 0;
|
|
|
|
void main()
|
|
{
|
|
int k;
|
|
char tmp[32];
|
|
char piste_c[8];
|
|
short couleur = 0;
|
|
|
|
//Initialise tous les peripheriques, taches et cycliques handler
|
|
ports_mcu();
|
|
lcd_init();
|
|
periph_init();
|
|
periph_nom("#Camobile*");
|
|
can_init();
|
|
clavier_init(1);
|
|
capture_init();
|
|
sta_tsk(ID_periph_rx);
|
|
|
|
sta_tsk(ID_compteur);
|
|
sta_cyc(ID_charge);
|
|
|
|
dly_tsk(100);
|
|
sta_tsk(ID_asserv_tourelle);
|
|
|
|
//Obtention de la couleur du feu
|
|
while ( (send_requete(MODE_COURSE)>>15)==0 ){ //Attente que le feu soit vert //**
|
|
dly_tsk(50);
|
|
}
|
|
|
|
sta_tsk(ID_asserv_roue);
|
|
for(k=1 ; k<=vit_max ; k++) {//*
|
|
vit_roue = k;//**
|
|
dly_tsk(100);
|
|
}
|
|
|
|
couleur_piste();
|
|
sta_tsk(ID_strategie);
|
|
|
|
switch(num_piste){ //vitesse initiale en fonction de la piste
|
|
case PISTE_VERTE :
|
|
last_vitesse = 40;
|
|
sprintf(piste_c,"P:Vert \0");
|
|
break;
|
|
case PISTE_BLEUE :
|
|
last_vitesse = 30;
|
|
sprintf(piste_c,"P:Bleu \0");
|
|
break;
|
|
case PISTE_ROUGE :
|
|
last_vitesse = vit_max_rouge;
|
|
sprintf(piste_c,"P:Rouge\0");
|
|
break;
|
|
case PISTE_NOIRE :
|
|
last_vitesse = 25;
|
|
sprintf(piste_c,"P:Noir \0");
|
|
break;
|
|
}
|
|
lcd_com(0x80);
|
|
lcd_str(piste_c);
|
|
|
|
//code_secret = 0x0123;
|
|
//send_commande('j',code_secret);
|
|
//code_secret=-1;
|
|
while(1){
|
|
/*
|
|
if (code_secret<0){
|
|
code_secret = send_requete('j');
|
|
code_secret=code_secret*2;
|
|
}
|
|
else{
|
|
code_secret = 0x0123;
|
|
send_commande('J',code_secret);
|
|
}
|
|
*/
|
|
|
|
|
|
lcd_cls();
|
|
|
|
couleur = capteur & 0xFF00;
|
|
switch(couleur){
|
|
case 0x4200 :
|
|
sprintf(tmp,"%s C:Cyan Tr %d Char %.0f%%",piste_c,nbre_tours,(float)charge_cpu);
|
|
break;
|
|
case 0x6300 :
|
|
sprintf(tmp,"%s C:Cyan Tr %d Char %.0f%%",piste_c,nbre_tours,(float)charge_cpu);
|
|
break;
|
|
case 0x4300 :
|
|
sprintf(tmp,"%s C:Bleu Tr %d Char %.0f%%",piste_c,nbre_tours,(float)charge_cpu);
|
|
break;
|
|
case 0x5200 :
|
|
sprintf(tmp,"%s C:RougeTr %d Char %.0f%%",piste_c,nbre_tours,(float)charge_cpu);
|
|
break;
|
|
case 0x4A00 :
|
|
sprintf(tmp,"%s C:JauneTr %d Char %.0f%%",piste_c,nbre_tours,(float)charge_cpu);
|
|
break;
|
|
case 0x5600 :
|
|
sprintf(tmp,"%s C:Vert Tr %d Char %.0f%%",piste_c,nbre_tours,(float)charge_cpu);
|
|
break;
|
|
|
|
}
|
|
lcd_com(0x80);
|
|
lcd_str(tmp);
|
|
|
|
dly_tsk(100);
|
|
|
|
}
|
|
}
|
|
|
|
void strategie(VP_INT stacd){
|
|
static int k = 0;
|
|
static char derivee = 1; //si 1 : croissance, si -1 decroissance
|
|
|
|
while(1){
|
|
if(Bp_G==1){
|
|
ter_tsk(ID_asserv_roue);
|
|
send_commande('V',0);
|
|
send_commande('D',0);
|
|
|
|
LED_R=1;
|
|
|
|
BR_appuye = 1;
|
|
|
|
}
|
|
else {
|
|
if(BR_appuye == 1){ //le bouton ete appuye
|
|
sta_tsk(ID_asserv_roue);
|
|
BR_appuye = 0;
|
|
}
|
|
LED_R=0;
|
|
vit_roue = last_vitesse;
|
|
rotation_roues_strat = 1; //angle des roues libres
|
|
|
|
|
|
capteur = send_requete(INFO_CAPTEUR);//**
|
|
switch(num_piste){
|
|
case PISTE_VERTE :
|
|
switch(capteur){
|
|
case 0x5601 : //Feu
|
|
vit_roue = vit_max_verte;
|
|
if(k==0){k=20;}
|
|
if(k<vit_max_verte/2){vit_roue = k*2;k++;}
|
|
K_roue = 1;
|
|
pseudo_sem = 1;
|
|
if(nbre_tours == 4){
|
|
ter_tsk(ID_main);
|
|
ter_tsk(ID_asserv_tourelle);
|
|
ter_tsk(ID_asserv_roue);
|
|
send_commande('V',0);
|
|
send_commande('D',0);
|
|
dly_tsk(200);
|
|
ter_tsk(ID_periph_tx);
|
|
ter_tsk(ID_periph_rx);
|
|
}
|
|
break;
|
|
|
|
case 0x4A01 : //Entree 1er virage
|
|
vit_roue = vit_virage_verte;
|
|
K_roue = 1.8;
|
|
compar_mur = 680;
|
|
derivee = 1;
|
|
k = vit_max_verte;
|
|
break;
|
|
case 0x5602 : //Fin 1er virage
|
|
switch(derivee){
|
|
case 1 :
|
|
k++;
|
|
if (k == 90){derivee = -1;}
|
|
break;
|
|
case -1 :
|
|
k--;
|
|
if (k == vit_max_verte){derivee = 0;}
|
|
break;
|
|
}
|
|
vit_roue = k;
|
|
K_roue = 1;
|
|
compar_mur = 680;
|
|
break;
|
|
case 0x5202 : //Entree 2eme virage
|
|
vit_roue = vit_virage_verte;
|
|
K_roue = 1.8;
|
|
compar_mur = 680;
|
|
derivee = 1;
|
|
k = vit_max_verte;
|
|
break;
|
|
case 0x5603 : //Fin 2eme virage
|
|
switch(derivee){
|
|
case 1 :
|
|
k++;
|
|
if (k == 90){derivee = -1;}
|
|
break;
|
|
case -1 :
|
|
k--;
|
|
if (k == vit_max_verte){derivee = 0;}
|
|
break;
|
|
}
|
|
vit_roue = k;
|
|
K_roue = 1;
|
|
compar_mur = 680;
|
|
break;
|
|
case 0x4203 : //Entree 3eme virage
|
|
vit_roue = vit_virage_verte;
|
|
K_roue = 1.8;
|
|
compar_mur = 680;
|
|
derivee = 1;
|
|
k = vit_max_verte;
|
|
break;
|
|
case 0x5604 : //Fin 3eme virage
|
|
switch(derivee){
|
|
case 1 :
|
|
k++;
|
|
if (k == 90){derivee = -1;}
|
|
break;
|
|
case -1 :
|
|
k--;
|
|
if (k == vit_max_verte){derivee = 0;}
|
|
break;
|
|
}
|
|
vit_roue = k;
|
|
K_roue = 1;
|
|
compar_mur = 680;
|
|
break;
|
|
|
|
case 0x4304 : //Entree 4eme virage
|
|
vit_roue = vit_virage_verte;
|
|
K_roue = 1.8;
|
|
compar_mur = 680;
|
|
if (pseudo_sem == 1){
|
|
nbre_tours++;
|
|
pseudo_sem = 0;
|
|
}
|
|
derivee = 1;
|
|
k = vit_max_verte;
|
|
break;
|
|
|
|
case 0x5605 : //Fin 4eme virage
|
|
switch(derivee){
|
|
case 1 :
|
|
k++;
|
|
if (k == 90){derivee = -1;}
|
|
break;
|
|
case -1 :
|
|
k--;
|
|
if (k == vit_max_verte){derivee = 0;}
|
|
break;
|
|
}
|
|
vit_roue = k;
|
|
K_roue = 1;
|
|
compar_mur = 680;
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case PISTE_BLEUE :
|
|
switch(capteur){
|
|
case 0x5601 : // Feu
|
|
vit_roue = 50;
|
|
if(k==0){k=20;}
|
|
if(k<vit_max_verte/2){vit_roue = k*2;k++;}
|
|
pseudo_sem = 1;
|
|
if(nbre_tours == 4){
|
|
ter_tsk(ID_main);
|
|
ter_tsk(ID_asserv_tourelle);
|
|
ter_tsk(ID_asserv_roue);
|
|
send_commande('V',0);
|
|
send_commande('D',0);
|
|
dly_tsk(200);
|
|
ter_tsk(ID_periph_tx);
|
|
ter_tsk(ID_periph_rx);
|
|
}
|
|
break;
|
|
|
|
case 0x4A01 : //Entree 1er virage
|
|
vit_roue = 30;
|
|
derivee = 1;
|
|
k = vit_max_verte;
|
|
break;
|
|
|
|
case 0x4304 : // Entree 4eme virage
|
|
if (pseudo_sem == 1){
|
|
nbre_tours++;
|
|
pseudo_sem = 0;
|
|
}
|
|
derivee = 1;
|
|
k = vit_max_verte;
|
|
break;
|
|
|
|
case 0x5605 : //Fin 4eme virage
|
|
switch(derivee){
|
|
case 1 :
|
|
k++;
|
|
if (k == 90){derivee = -1;}
|
|
break;
|
|
case -1 :
|
|
k--;
|
|
if (k == vit_max_verte){derivee = 0;}
|
|
break;
|
|
}
|
|
vit_roue = k;
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case PISTE_ROUGE :
|
|
switch(capteur){
|
|
case 0x4A01 : //Capteur avant virage ou route a trou ou il faut etre centre
|
|
vit_roue = 32;
|
|
break;
|
|
|
|
case 0x5202 : //Capteur apres route a trou ou il faut etre centre
|
|
vit_roue = vit_max_rouge;
|
|
break;
|
|
|
|
case 0x6301 : //Capteur perso avant bosse rouge
|
|
pseudo_sem = 1;
|
|
rotation_roues_strat = 0; //blocage de langle des roues
|
|
vit_roue = 45;
|
|
break;
|
|
|
|
case 0x6302 : //Capteur perso juste apres bosse rouge
|
|
rotation_roues_strat = 1;
|
|
vit_roue = 8;
|
|
k = 0;
|
|
break;
|
|
|
|
case 0x6304 : // Capteur perso apres bosse rouge
|
|
if (k == 0){
|
|
k = 8*9;
|
|
}
|
|
if(k < (vit_max_rouge*9)){
|
|
vit_roue = k/9;
|
|
k++;
|
|
}
|
|
break;
|
|
|
|
case 0x5604 : //Fin 3eme virage
|
|
if(k < (vit_max_rouge*9) ){
|
|
vit_roue = k/9;
|
|
k++;
|
|
}
|
|
break;
|
|
|
|
case 0x4304 : // Capteur avant fin
|
|
vit_roue = vit_max_rouge;
|
|
if (pseudo_sem == 1){
|
|
nbre_tours++;
|
|
pseudo_sem = 0;
|
|
}
|
|
break;
|
|
|
|
case 0x5601 : // Capteur debut de piste
|
|
vit_roue=40;
|
|
if(nbre_tours == 4){
|
|
ter_tsk(ID_main);
|
|
ter_tsk(ID_asserv_tourelle);
|
|
ter_tsk(ID_asserv_roue);
|
|
send_commande('V',0);
|
|
send_commande('D',0);
|
|
dly_tsk(200);
|
|
ter_tsk(ID_periph_tx);
|
|
ter_tsk(ID_periph_rx);
|
|
}
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case PISTE_NOIRE :
|
|
switch(capteur){
|
|
case 0x5601 : //Feu
|
|
if(nbre_tours == 4){
|
|
ter_tsk(ID_main);
|
|
ter_tsk(ID_asserv_tourelle);
|
|
ter_tsk(ID_asserv_roue);
|
|
send_commande('V',0);
|
|
send_commande('D',0);
|
|
dly_tsk(200);
|
|
ter_tsk(ID_periph_tx);
|
|
ter_tsk(ID_periph_rx);
|
|
}
|
|
else{
|
|
pseudo_sem = 1;
|
|
compar_mur = 680; //decalage a gauche
|
|
filtre_sup = 1500;
|
|
filtre_inf = 400;
|
|
}
|
|
break;
|
|
|
|
case 0x4a01 : //Entree 1er virage
|
|
compar_mur = 500; //voiture centree
|
|
filtre_sup = 1000;
|
|
filtre_inf = 0;
|
|
break;
|
|
|
|
case 0x6302 : //PERSO : moitie montee bosse
|
|
vit_roue = 14;
|
|
filtre_inf = 470;
|
|
filtre_sup = 530;
|
|
break;
|
|
|
|
case 0x6380 : //PERSO : moitie descente bosse
|
|
filtre_inf = 0;
|
|
filtre_sup = 1000;
|
|
vit_roue = vit_max;
|
|
break;
|
|
|
|
case 0x6320 : //PERSO : avant saut
|
|
rotation_roues_strat = 0; //blocage de langle des roues
|
|
vit_roue = 42;
|
|
break;
|
|
|
|
case 0x6304 : //PERSO : apres saut
|
|
vit_roue = 8;
|
|
filtre_sup = 1600;
|
|
rotation_roues_strat = 1; //deblocage de langle des roues
|
|
K_roue = 1.0;
|
|
break;
|
|
|
|
case 0x63dd : //PERSO : juste avant fin 3eme virage
|
|
filtre_inf = 100;
|
|
filtre_sup = 900;
|
|
vit_roue = 12;
|
|
K_roue = 1.0;
|
|
break;
|
|
|
|
case 0x5604 : //Fin 3eme virage
|
|
filtre_inf = 100;
|
|
filtre_sup = 900;
|
|
vit_roue = 12;
|
|
K_roue = 1.0;
|
|
break;
|
|
|
|
case 0x63e0 : //Juste avant 4eme virage
|
|
filtre_inf = 0;
|
|
filtre_sup = 1000;
|
|
vit_roue = vit_max;
|
|
break;
|
|
|
|
case 0x4304 : //Entree 4eme virage
|
|
filtre_inf = 0;
|
|
filtre_sup = 1000;
|
|
vit_roue = vit_max;
|
|
if (pseudo_sem == 1){
|
|
nbre_tours++;
|
|
pseudo_sem = 0;
|
|
}
|
|
break;
|
|
}//END_SWITCH_capteur
|
|
break;
|
|
}//END_SWITCH_couleur
|
|
if(vit_roue != 0){
|
|
last_vitesse = vit_roue;
|
|
}
|
|
}//END_ELSE
|
|
|
|
|
|
dly_tsk(10);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
void maj_variables(void){
|
|
if (Bp_G == 1){LED_R = 1;} //si bouton poussoire gauche appuy? allumer led rouge
|
|
else{LED_R = 0;}
|
|
|
|
//Angle des roues necessaire pour toutes les pistes
|
|
distance_mur = distance_telemetre()*sin(3.1415*angle_tourelle/1800.0);
|
|
if(distance_mur > filtre_sup || distance_mur < filtre_inf) { //Trou dans le mur
|
|
rotation_roues_asserv = 0;
|
|
}
|
|
else {
|
|
rotation_roues_asserv = 1;
|
|
}
|
|
ang_roue = rotation_roues_asserv*rotation_roues_strat*(K_roue*(distance_mur-compar_mur));
|
|
}
|
|
|
|
void asserv_roue(VP_INT stacd){
|
|
//Maintient de la voiture au centre de la piste
|
|
while(1){
|
|
maj_variables();//***
|
|
|
|
angle_roue(ang_roue);
|
|
vitesse_roue(vit_roue);//****
|
|
|
|
dly_tsk(10);//**50
|
|
}
|
|
}
|
|
|
|
void asserv_tourelle(VP_INT stacd){
|
|
//Maintient de la tourelle orientee a 45 degre
|
|
CanFrame comm;
|
|
unsigned int vitesse_tourelle;
|
|
char K = 2;
|
|
|
|
while(1){
|
|
vitesse_tourelle = K*(angle_tourelle - position_tourelle());
|
|
send_commande(VITESSE_TOURELLE, vitesse_tourelle);
|
|
dly_tsk(10);//**
|
|
}
|
|
}
|
|
|
|
|
|
void couleur_piste(void){
|
|
//Obtention de la piste
|
|
num_piste = send_requete(MODE_COURSE) & 0x00FF;
|
|
}
|
|
|
|
void rotation_tourelle(unsigned int angle){
|
|
send_commande(VITESSE_TOURELLE,angle);
|
|
}
|
|
|
|
int position_tourelle(){
|
|
//Renseigne sur la position de la tourelle (angle en dizieme de degre)
|
|
int angle;
|
|
|
|
angle = send_requete(ANGLE_TOURELLE);
|
|
return angle;
|
|
}
|
|
|
|
short distance_telemetre(void){
|
|
//Renseigne la distance mesuree par le telemetre
|
|
short distance;
|
|
|
|
distance = send_requete(DISTANCE_TELEMETRE);
|
|
return distance;
|
|
}
|
|
|
|
void vitesse_roue(unsigned int vitesse){
|
|
send_commande(VITESSE_ROUES,vitesse);
|
|
}
|
|
|
|
void angle_roue(unsigned int angle){
|
|
send_commande(ANGLE_ROUES,angle);
|
|
}
|
|
|
|
unsigned int send_requete(unsigned char id_periph){//**
|
|
//Envoi la requete correspondant a lidentifiant id_periph
|
|
CanFrame req;
|
|
unsigned int val;
|
|
UINT flag;
|
|
|
|
req.data.id = id_periph;
|
|
req.data.rtr = 1; // envoie d'une requete de lecture
|
|
snd_dtq(CanTx, req.msg); // attente de la reponse
|
|
dly_tsk(10);
|
|
|
|
val = periph[ADDR(id_periph)].val;
|
|
return val;
|
|
}
|
|
|
|
void send_commande(unsigned char id_periph, unsigned int val){//**
|
|
//Envoi la commande correspondant a lidentifiant id_periph avec la valeur val
|
|
CanFrame comm;
|
|
|
|
comm.data.id = id_periph;
|
|
comm.data.rtr = 0; //Ecriture
|
|
comm.data.val = val;
|
|
snd_dtq(CanTx, comm.msg);
|
|
}
|
|
|
|
void compteur(VP_INT stacd){
|
|
//Compte quand le cpt ne fait rien d'autre
|
|
//ter_tsk(ID_main);
|
|
//ter_tsk(ID_periph_tx);
|
|
while(1){
|
|
cpt_cpu++;
|
|
}
|
|
}
|
|
|
|
void charge()
|
|
{
|
|
charge_cpu = 100 - (cpt_cpu*100)/1156668;
|
|
|
|
//sif(cpt_cpu_max<cpt_cpu) cpt_cpu_max = cpt_cpu;
|
|
cpt_cpu =0;
|
|
}
|
|
|