#include <tracknew.h>
#include <stdlib.h>
#include <LCD.C>

#define LEFT (PIN_A3)     // signal de commande left
#define RIGHT (PIN_A2)    // signal de commande right
#define DOWN (PIN_A0)     // signal de commande down
#define UP (PIN_A1)       // signal de commande up
#define CMD_LEFT (PIN_C3) // Poussoir de commande left
#define CMD_RIGHT (PIN_C0)// Poussoir de commande right
#define CMD_DOWN (PIN_C4) // Poussoir de commande down
#define CMD_UP (PIN_C5)   // Poussoir de commande up
#define AZ (PIN_B5)       // signal PULSE d'azimut
#define EL (PIN_B4)       // signal PULSE de site

#define SPEED_EL (PIN_C1)
#define SPEED_AZ (PIN_C2)
#define PARK (PIN_E0)
#define MEMO_PARK (PIN_E1)
#define ZERO_PARK (PIN_E2)
#byte pulse_el = 4
#byte pulse_az = 5

#byte port_b=6
#byte port_c=7
#byte port_e=9

#priority RB, EXT, RDA

#use fast_io(C)
#use fast_io(B)

// ******************** variables globales ********************************//

static int16 nb_imp_az, nb_imp_el;         // nombre d'impulsions
static int16 cible_site;          // valeur site demandée
static int16 cible_azimut;        // valeur azimut demandée
static int16 position_site;          // position site actuelle
static int16 position_azimut;        // position azimut actuelle
static int16 ecart_depart, ecart_arret;
static int up_on;
static int down_on;
static int left_on;
static int right_on;
static int tempo;
static int16 table_site[19];
static char rcv[60];
int a=0,b=0;
static int16 cible_site_impulsion;
static int16 cible_azimut_impulsion;
static int el_motor_rq=0;
static int az_motor_rq=0;
static int len=0,ref_az=100,ref_el=100;
static int8 last_B=0;
static int32 depart_az;
static int track=32;
static int16 raz_track=800;

#int_EXT
EXT_isr()

{
   // Power Fail, mise hors tension
   output_low(DOWN);
   output_low(UP);
   output_low(LEFT);
   output_low(RIGHT);
   disable_interrupts(GLOBAL); // all interrupts OFF
   delay_us(10);
	WRITE_EEPROM(4,nb_imp_az>>8);
   delay_us(10);
   WRITE_EEPROM(5,nb_imp_az);
   delay_us(10);
	WRITE_EEPROM(2,nb_imp_el>>8);
   delay_us(10);
   WRITE_EEPROM(3,nb_imp_el);
   delay_us(10);
   sleep();
   sleep();
}



#int_RB
RB_isr()
// Routine d'interruption sur chaque changement sur le port B
// Compte ou décompte les changements d'états des entrées B4 et B5
// selon le sens de rotation des moteurs .
// Deux changements d'état par impulsion.
{
int8  change;
int16 i;
// lecture du port B et isolement des deux bits B4 et B5
change = (port_b & 0b00110000) ^ last_B; // Identification des changements
last_B = port_b;  // Rafraichissement de la position du port

if( bit_test(change,5)) // debut imp AZ
   {
    if(left_on==1)
      nb_imp_az-=1;
   if(right_on==1)
      nb_imp_az+=1;
   }
if( bit_test (change, 4))
   {
   if(down_on==1)
      nb_imp_el-=1;
   if(up_on==1)
      nb_imp_el+=1;
   }
for (i=0;i<10000;i=i+2){i--;}
}

//////////////////////////////
// RS232 receive interrupt
#int_RDA
void RDA_isr()
{
char a;
a=getc();
if (isprint(a) || a==13 )
   rcv[len++]=a ;
}

void ecrire_eeprom(int ad,int16 don){
// *********************************************************************** //
// *********Ecrit un mot de 16 BITS dans l'eeprom à l'adresse ad********** //
   disable_interrupts(GLOBAL); // all interrupts OFF
	WRITE_EEPROM(ad,don>>8);
   WRITE_EEPROM(ad+1,don);
   enable_interrupts(GLOBAL); // all interrupts ON
}


int16 lire_eeprom(int16 adresse)
// Lit dans l'eeprom un mot de 16 BITS
{
int16 donne=0;
   donne=read_eeprom(adresse);
   delay_ms(5);
   donne=((donne<<8)|(read_eeprom(adresse+1)));
   return(donne);
}


#SEPARATE
void table (void)
// Procédure de création d'une table de site
{

int a,t;
int16 table[19]=100,161,225,291,356,420,483,545,606,666,725,783,835,885,928,969,1007,1037,1045;
//int16 table[19]=100,190,288,388,486,582,674,766,856,944,1030,1114,1196,1275,1350,1419,1479,1520,1521;

if (read_eeprom(6)==55) return;
for (a=20,t=0;t<20;a=a+2,t=t+1)
   {
	ecrire_eeprom(a,table[t]);
   }
write_eeprom(6,55);

}

#SEPARATE
int16 GetNbDegre(Int16 iImpulsion)
//********************* VERIN DE SITE *****************************************************
// Procédure qui donne la correspondance en degrés de la valeur du compteur d'impulsion.
// Recherche dans la table d'étalonage les valeurs de compteur les plus proche pour déterminer
// le ratio degré/impulsions à appliquer
{
Int16 borneInf, nbDegre, nbImpulsionParDegre;
if (iImpulsion>=table_site[18])
     return (90);

borneInf=0;
nbDegre=0;
while (borneInf<18)
   {
   if (iImpulsion<table_site[0])  return (0);
   if (iImpulsion<table_site[borneInf])
      {
       borneInf--;
       break;
      }
      borneInf++;
    }

if (borneInf>=18)
   borneInf--;
nbImpulsionParDegre=(table_site[borneInf+1]-table_site[borneInf])/5;
nbDegre=(borneInf*5)+(iImpulsion-table_site[borneInf])/nbImpulsionParDegre;
return nbDegre;
}

#SEPARATE
Int16 GetNbImpulsion(Int16 iDegre)
//********************* VERIN DE SITE *****************************************************
// Procédure qui retourne la valeur du compteur d'impulsion correspondant à une position angulaire en degrés
{
    Int16 borneInf, ecartDegre, nbImpulsion, nbImpulsionParDegre;
    borneInf=iDegre/5; // division entière pour obtenir la borne inférieure dans table_size
    ecartDegre=iDegre-(borneInf * 5);
    nbImpulsionParDegre=(table_site[borneInf+1]-table_site[borneInf])/5;
    nbImpulsion=table_site[borneInf]+(ecartDegre*nbImpulsionParDegre);
    return nbImpulsion;
}

#SEPARATE
void test_position(int offset)
{
   if (nb_imp_az>=ref_az)
         position_azimut=nb_imp_az-ref_az;
   if (nb_imp_az<ref_az)
         position_azimut=ref_az-nb_imp_az;
   if (cible_azimut > (position_azimut+offset) || (cible_azimut+10) < ((position_azimut+10)-offset) )      // Si changement de position détectée > 2°
            {
            az_motor_rq=1;    // Demande de démarrage moteur d'azimut
            depart_az=position_azimut;
            }
   else
            az_motor_rq=0;

   position_site=GetNbDegre(nb_imp_el);
   if (cible_site > (position_site+offset) || (cible_site+10) < ((position_site+10)-offset) )     // Si changement de position détectée > 2°
            el_motor_rq=1;    // Demande de démarrage moteur de site
   else
            el_motor_rq=0;

}

#SEPARATE
void format_affiche (int16 nb_4_digit)
// Procédure qui affiche sur l'écran LCD à la position courante, un integer sur 4 digits
// Les zéros non sigificatifs ne sont pas affichés
   {
   int position_millier;
   int position_centaine;
   int position_dizaine;
   int position_unite;
   position_millier=nb_4_digit / 1000;
   position_centaine=(nb_4_digit /100)%10;
   position_dizaine=(nb_4_digit / 10)%10;
   position_unite=nb_4_digit % 10;
   if (position_millier!=0) lcd_putc(position_millier+48);  else lcd_putc(" ");
   if (position_millier==0 && position_centaine == 0) lcd_putc(" "); else lcd_putc(position_centaine+48);
   if (position_millier==0 && position_centaine == 0 && position_dizaine == 0) lcd_putc(" "); else lcd_putc(position_dizaine+48);
   lcd_putc(position_unite+48);
   }

#SEPARATE
void affiche_position (int ligne, int16 val_site , int16 val_azimut)
// Procédure qui affiche sur l'écran LCD les positions d'antennes
// Sur la première ligne : la consigne à atteindre ('Req'uest)
// Sur la deuxième ligne : la position instantanée des moteurs ('Pos'ition)
      {
      lcd_gotoxy(1,ligne);
      lcd_putc("AZ");
      format_affiche(val_azimut);
      lcd_putc(0xdf);
      lcd_putc(" EL");
      format_affiche (val_site);
      lcd_putc(0xdf);
      lcd_gotoxy(18,1);
      lcd_putc("Req");
      lcd_gotoxy(18,2);
      lcd_putc("Pos");
      }

#SEPARATE
void config_az(void)
{
int16 set_azimut=0;
int i,j;
   disable_interrupts(GLOBAL); // all interrupts OFF
	lcd_gotoxy(1,2);
   lcd_putc("THEN PUSH 'PARK'");
   lcd_gotoxy(1,1);
   lcd_putc("REF AZIMUT ");
	format_affiche(set_azimut);

	while (input(PARK)==1)
	{
   i=1000,j=1;
   while ((input(CMD_RIGHT)==0) && (input(CMD_LEFT)==1 ))
      {
      if (set_azimut < 780 )  // Deux impulsions par degré, 720 imp pour 360°
			set_azimut+=1;
		delay_ms(i);
		j=++j;
		if (i>30){ i=i-(20*j);}
      lcd_gotoxy(12,1);
		format_affiche(set_azimut/2);
      }
   while ((input(CMD_LEFT)==0) && (input(CMD_RIGHT)==1))
      {
      if (set_azimut > 0 )
			set_azimut-=1;
		delay_ms(i);
		j=++j;
		if (i>30){ i=i-(20*j);}
      lcd_gotoxy(12,1);
		format_affiche(set_azimut/2);
      }

   }
   lcd_gotoxy(1,1);
   lcd_putc(" SET AZIMUT TO ");
   format_affiche(set_azimut/2);
   delay_ms(800);
  	while (input(PARK)==1)
	{

		while (input(CMD_LEFT)==0||input(CMD_RIGHT)==0)
   		{
	    	if (input(CMD_RIGHT)==0)
            output_high(RIGHT);
   		if (input(CMD_LEFT)==0)
            output_high(LEFT);
	      }
         output_low(LEFT);
	      output_low(RIGHT);
		}
      nb_imp_az=set_azimut+ref_az;
      ecrire_eeprom(4,nb_imp_az);
	   delay_ms(800);

   lcd_gotoxy(1,1);
   lcd_putc("SET ELEV. TO ZERO   ");
	lcd_gotoxy(1,2);
   lcd_putc("THEN PUSH 'PARK'");

while (input(PARK)==1)
   {
	while (input(CMD_UP)==0||input(CMD_DOWN)==0)
		{
		if (input(CMD_UP)==0)
        	output_high(UP);
  		if (input(CMD_DOWN)==0)
  			output_high(DOWN);
      }
    	output_low(DOWN);
      output_low(UP);
   }
   ecrire_eeprom(2,ref_el);
   delay_ms(1000);
   reset_cpu();
}
#SEPARATE
void config_el (void)
{
	int a;
   int16 c;
   nb_imp_el=ref_el;
   lcd_gotoxy(1,1);
   lcd_putc("  CONFIG ELEVATION  ");
   lcd_gotoxy(1,2);
   lcd_putc("Push 'PARK' when Ok ");
	while (input(PARK)==1)
         a=0;
   delay_ms(1000);
   lcd_gotoxy(1,1);
   lcd_putc("Push 'PARK' when Ok ");
   for (a=20,c=0;c<=90;a=a+2,c=c+5)
		{
		while (input(PARK)==1)
			{
         lcd_gotoxy(1,2);
         lcd_putc("EL TO");
         format_affiche (c);

      	while (input(CMD_UP)==0||input(CMD_DOWN)==0)
		      {
      		if (input(CMD_UP)==0)
		     	   {
			      up_on=1;
			      output_high(UP);
			      }
		      if (input(CMD_DOWN)==0)
			      {
			      down_on=1;
			      output_high(DOWN);
			      }
	         }
 	       output_low(DOWN);
          output_low(UP);
	       up_on=0;down_on=0;
          lcd_putc("  IMP ");
          format_affiche (nb_imp_el);
			 }
      delay_ms(500);
		ecrire_eeprom(a,nb_imp_el);
		}
      ecrire_eeprom(2,nb_imp_el);
      write_eeprom(6,55);
		delay_ms(1000);
reset_cpu();
}


#SEPARATE
void test_memo()
{
if (port_E  ^ 0x07)
{
	if(input(park)==0 && input(memo_park)==1)		// Positionne les antennes à la valeur de park
	{
	cible_site=lire_eeprom(10);
	cible_azimut=lire_eeprom(8);
   if (cible_site > 90) cible_site=0;
   if (cible_azimut > 780) cible_azimut = 0;    // Deux impulsions par degré, 720 imp pour 360°
   affiche_position(1, cible_site, cible_azimut/2);
   lcd_gotoxy(16,1);
   lcd_putc("P");
   test_position(0);
	return;
	}

	if(input(memo_park)==0 && input(park)==0)	// Mémorise les positions actuelles comme valeurs de park
	{
	ecrire_EEPROM(8,position_azimut);
	ecrire_EEPROM(10,position_site);
   lcd_gotoxy(1,1);
   lcd_putc(" PARK MEMORISE   ");
	return;
	}

	if(input(zero_park)==0)		// Positionne les antennes à 0° AZ et 0° EL
	{
	cible_site=0;cible_azimut=0;
	affiche_position(1, cible_site, cible_azimut/2);
   lcd_gotoxy(16,1);
   lcd_putc("Z");
   test_position(0);
	return;
	}
}
}

#SEPARATE
void lecture_table(void)
// Procédure pour lire la table des impulsions de site
// A la mise sous tension si pousoir "PARK ZERO"

{
int16 imp_el;
int a,b,c;
   for (a=20,b=0,c=0;b<=18;a=a+2,b++,c=c+5)
      {				// Boucle de lecture de la table de correspondance
       imp_el=lire_eeprom(a);					// impulsions <-> degrés

       lcd_gotoxy(1,1);
       lcd_putc(" LECTURE TABLE SITE ");
       lcd_gotoxy(1,2);
       lcd_putc("EL");
       format_affiche (c);
       lcd_putc(0xdf);
       lcd_putc(" IMP ");
       format_affiche(imp_el);
       while (input(zero_park)==1)
          {
          delay_ms(1);
          }
        delay_ms(1000);
      }
}


#SEPARATE
int extract (void)
// Procédure qui extrait les deux premières valeurs entières de la chaine rcv
// La première est la valeur de l'azimut, la seconde le site.
{
int	i,	k,	iWord=0;
char	szValue1[4], szValue2[4];
int	bWord=false,	bDrop=false;
	  memset(szValue1,0,sizeof(szValue1));
	  memset(szValue2,0,sizeof(szValue2));
	  for(i=0,k=0;i<strlen(rcv);i++) {
		if(rcv[i]=='.') {
			bWord=false;
			bDrop=true;
			continue;
		}
		if(rcv[i]==' ') {
			bWord=false;
			bDrop=false;
			continue;
		}
		if(rcv[i]<0x30 || rcv[i]>0x39) {
			continue;
		}
   	if(bDrop==true) {
			continue;
		}
		if(bWord==false) {
			k=0;
			iWord++;
			bWord=true;
		}
		switch(iWord) {
			case 1 :
				szValue1[k]=rcv[i];
				k++;
				break;
			case 2 :
				szValue2[k]=rcv[i];
				k++;
				break;
			default :
				break;
		}
	  }
   // traitement des valeurs incohérentes
   if (atol(szValue1)<0 || atol(szValue1)>360 ) return(FALSE);
   if (atol(szValue2)<0 || atol(szValue2)>90 ) return(FALSE);
   cible_azimut=atol(szValue1)*2; // Deux impulsions par degré, 720 imp pour 360°
	cible_site=atol(szValue2);
return (TRUE);
}
#SEPARATE
void trt_rs232(void)
// Traitement de la chaine reçue par l'UART en RS232 au protocole GS232
// Format GS232 : Commande "W" -> Waaa eee(rc) aaa= azimut sur 3 digits eee= élévation sur 3 digits.
//						Commande "M" -> Maaa(rc) aaa = azimut
// Format EazyComm : AZaaa.a ELee.e
// Format Nova etc..
// Plus généralement, cette fonction extrait les deux premières valeurs entières de la chaîne reçue
// la première étant l'azimut, la seconde le site
{
char string1[4];
char string2[4];

if (rcv[len-1]==13)
   {
   rcv[len-1]='\0';
   strcpy(string1,"AZ");
   strcpy(string2,"EL");

   if ((strstr(rcv,string1) && strstr(rcv,string2)) ||(rcv[0]=='M' ) || (rcv[0]=='W'))
      {
      if (extract())
         {
         raz_track=0;
         track='T'; // "T"
         lcd_gotoxy(16,1);
         lcd_putc(track);
	      affiche_position (1, cible_site , cible_azimut/2);
         test_position(2);
         }
      }
   len=0;
   rcv[0]='\0';
   }
}




// *********************************************************************** //
// Procédure qui gère la mise en route et l'arret du moteur de site.
// en fonction :
// - De la position effective du moteur ou du vérin : variable position_site
// - De la valeur de la consigne à atteindre : variable cible_site

void moteur_site (void)
{
position_site=GetNbDegre(nb_imp_el);
cible_site_impulsion=GetNbImpulsion(cible_site);
if (down_on && (nb_imp_el <= cible_site_impulsion)) // Arret en descente
      {
      output_low(DOWN);
      affiche_position(2, position_site , position_azimut/2);//(cible_azimut/2)%360)
		delay_ms(1500);
      el_motor_rq=2;
      down_on=0;
		return;
      }

if (up_on && (nb_imp_el >= cible_site_impulsion))  // Arret en montée
      {
      output_low(UP);
      affiche_position(2, position_site , position_azimut/2);//(cible_azimut/2)%360)
		delay_ms(1500);
      el_motor_rq=2;
      up_on=0;
		return;
      }

if  (nb_imp_el > cible_site_impulsion)
      {
      if (up_on)
         {
         output_low(UP);
         delay_ms(1500); // Delai pour compter les impulsions dues à l'inertie du moteur
         up_on=0;
         }
      down_on=1;
      output_high(DOWN);
	   return;
	   }
if  (nb_imp_el < cible_site_impulsion)
      {
      if (down_on)
         {
         output_low(DOWN);
         delay_ms(1500); // Delai pour compter les impulsions dues à l'inertie du moteur
         down_on=0;
         }
   	up_on=1;
      output_high(UP);
	   return;
   	}
}

void moteur_azimut (void)
// Traitement commandes du moteur d'azimut
// en fonction de la position actuelle (position_azimut)
// de la consigne à atteindre (cible_azimut)

{
cible_azimut_impulsion=cible_azimut+ref_az;

if (nb_imp_az>=ref_az)
   position_azimut=nb_imp_az-ref_az;
if (nb_imp_az<ref_az)
   position_azimut=ref_az-nb_imp_az;

if ( right_on && (nb_imp_az >= cible_azimut_impulsion)) // Arret en marche avant
      {
      output_low(RIGHT);
     	affiche_position(2, position_site , position_azimut/2);//(cible_azimut/2)%360)
		delay_ms(1500); // Delai pour compter les impulsions dues à l'inertie du moteur
      az_motor_rq=2;
      right_on=0;
		return;
      }
if ( left_on && (nb_imp_az <= cible_azimut_impulsion)) // Arret en marche arrière
      {
      output_low(LEFT);
      affiche_position(2, position_site , position_azimut/2);//(cible_azimut/2)%360)
		delay_ms(1500); // Delai pour compter les impulsions dues à l'inertie du moteur
      az_motor_rq=2;
      left_on=0;
		return;
      }
if  (nb_imp_az > cible_azimut_impulsion)
      {
      if (right_on)
         {
         output_low(RIGHT);
         delay_ms(1500);
     	   right_on=0;
         }
      left_on=1;
      output_high(LEFT);
		return;
	   }
   if  (nb_imp_az < cible_azimut_impulsion)
	   {
      if (left_on)
         {
         output_low(LEFT);
         delay_ms(1500);
         left_on=0;
         }
      right_on=1;
      output_high(RIGHT);
      return;
	   }
}

#int_TIMER2
TIMER2_isr()
{
if (raz_track < 1010)
   raz_track++;
}


#int_TIMER1
TIMER1_isr()
{
tempo++;
}
#SEPARATE

void req_pos_ant (void)
// Procédure qui lit les poussoirs de commande pour renseigner les variable de consigne à atteindre
// en site et en azimut
// l'incrémentation des compteur est exponentielle lors de l'appui sur les poussoirs
{

if ((port_c & 0x39) ^ 0x39 && (raz_track > 1000))
// Contrôle des poussoirs de commande
// Si pas de tracking reçu depuis  13 secondes (1000 X 13 ms de T2)

{

// Arret des moteurs
   if (input(cmd_left)==0||input(cmd_right)==0 && az_motor_rq)
      {
      output_low(UP);
      output_low(DOWN);
      output_low(LEFT);
      output_low(RIGHT);
		delay_ms(1000); // Delai pour compter les impulsions dues à l'inertie du moteur
      az_motor_rq=2;
      right_on=0; left_on=0;
      cible_azimut = nb_imp_az-ref_az; position_azimut=cible_azimut;
      }
   if (input(cmd_up)==0||input(cmd_down)==0 && el_motor_rq)
      {
      output_low(LEFT);
      output_low(RIGHT);
      output_low(UP);
      output_low(DOWN);
		delay_ms(1000); // Delai pour compter les impulsions dues à l'inertie du moteur
      el_motor_rq=2;
      up_on=0; down_on=0;
      cible_site = GetNbDegre(nb_imp_el); position_site=cible_site;
      }
//recalcul des positions après arret des moteurs

     	affiche_position(2, position_site , position_azimut/2);//(position_azimut/2)%360)
     	affiche_position(1, cible_site , cible_azimut/2);//(cible_azimut/2)%360)

enable_interrupts(INT_TIMER1);
tempo=0;

while (tempo<12)
   {
   int i=500;
   int j=1;

   while ((input(CMD_UP)==0 ) && (input(CMD_DOWN)==1))
      {
		if (cible_site < 90)
			cible_site+=1;
		delay_ms(i);
		j=j++;
		if (i>=20){ i=i-(10*j*j);}
		affiche_position(1, cible_site, cible_azimut/2); //(cible_azimut/2)%360)
      tempo=0;
      }
   while ((input(CMD_DOWN)==0) && (input(CMD_UP)==1))
      {
      if (cible_site > 0 )
		cible_site-=1;
		delay_ms(i);
		j=j++;
		if (i>=20){ i=i-(10*j*j);}
		affiche_position(1, cible_site, cible_azimut/2);//(cible_azimut/2)%360)
      tempo=0;
      }
   while ((input(CMD_RIGHT)==0) && (input(CMD_LEFT)==1 ))
      {
      if (cible_azimut < 780 )   // Deux impulsions par degré, 720 imp pour 360°
			cible_azimut+=1;
		delay_ms(i);
		j=j++;
		if (i>=20){ i=i-(10*j*j);}
		affiche_position(1, cible_site, cible_azimut/2);//(cible_azimut/2)%360)
      tempo=0;
      }
   while ((input(CMD_LEFT)==0) && (input(CMD_RIGHT)==1))
      {
      if (cible_azimut > 0 )
			cible_azimut-=1;
		delay_ms(i);
		j=j++;
		if (i>=20){ i=i-(10*j*j);}
		affiche_position(1, cible_site, cible_azimut/2);//(cible_azimut/2)%360)
      tempo=0;
      }
   }
   disable_interrupts(INT_TIMER1);
   lcd_gotoxy(16,1);
   lcd_putc("M");
   test_position(0);
   }
}


void main()
{
	SET_TRIS_A(0x00);
   SET_TRIS_B(0xFF);          // Port B en entrée
   SET_TRIS_C(0xB9);
	SET_TRIS_E(0x07);

   output_low(UP);      // Place la sortie à 0
   output_low(DOWN);      // PLace la sortie à 0
   output_low(LEFT);      // PLace la sortie à 0
   output_low(RIGHT);      // PLace la sortie à 0
   output_low(SPEED_AZ);
   output_high(SPEED_EL);
   setup_counters(RTCC_INTERNAL,RTCC_DIV_256);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);  //104ms
   setup_timer_2(T2_DIV_BY_16,255,16);       // 13ms

   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_psp(PSP_DISABLED);
   setup_spi(FALSE);

	EXT_INT_EDGE(H_TO_L);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_RB);
   enable_interrupts(INT_RDA);
   disable_interrupts(INT_TIMER1);
   enable_interrupts(INT_TIMER2);
   enable_interrupts(global);

   lcd_init();                // initialisation du LCD
   lcd_putc("\f");            // Efface le LCD

// Configuration du controleur

 	table();  // création table de site

   if (input(PARK)==0)
      lecture_table();
	if (input(ZERO_PARK)==0)
		config_az();
	if (input(MEMO_PARK)==0)
		config_el();

   nb_imp_el=lire_eeprom(0x02);           // Lecture dans l'EEPROM du compteur d'impulsions de site
   for (a=20,b=0;b<=18;a=a+2,++b)				// Boucle de lecture de la table de correspondance
        table_site[b]=lire_eeprom(a);					// impulsions <-> degrés
   position_site=GetNbDegre(nb_imp_el);
   cible_site=position_site;

   nb_imp_az=(lire_eeprom(0x04));           // Lecture dans l'EEPROM du compteur d'impulsions d'azimut
if (nb_imp_az>=ref_az)
   position_azimut=nb_imp_az-ref_az;
if (nb_imp_az<ref_az)
   position_azimut=ref_az-nb_imp_az;
   cible_azimut=position_azimut; // Deux impulsions par degré, 720 imp pour 360°
   lcd_putc("\f");            // Efface le LCD
   lcd_gotoxy(5,1);
   lcd_putc("F1TE ROTOR");                //affichage sur le LCD du message d'accueil
   lcd_gotoxy(16,2);
   lcd_putc("*");

while (TRUE)
   {
	test_memo();     // Demande de positionnement programmé ?
	trt_rs232();     // Demande de positionnement ou de tracking via la COM ?
	req_pos_ant();   // Demande de positionnement par les poussoirs ?

 // Réduction de la vitesse azimut si écart < 8°
if (cible_azimut > position_azimut)
      {
      ecart_arret = cible_azimut - position_azimut;
      ecart_depart = position_azimut - depart_az;
      }
else
      {
      ecart_arret = position_azimut - cible_azimut;
      ecart_depart = depart_az - position_azimut ;
      }


   if (ecart_depart < 16 || ecart_arret < 16)
         output_low(speed_az);
   else
         output_high(speed_az);


   if (el_motor_rq==1)
      {
      lcd_gotoxy(16,2);
      lcd_putc(" ");
      disable_interrupts(INT_RDA);
     	moteur_site();
      }
   if (az_motor_rq==1)
      {
      lcd_gotoxy(16,2);
      lcd_putc(" ");
      disable_interrupts(INT_RDA);
      moteur_azimut();
      }

   if ((az_motor_rq | el_motor_rq)==2 )
      {
      if (el_motor_rq ==2)
         ecrire_eeprom(2,nb_imp_el);
      if (az_motor_rq ==2)
         ecrire_eeprom(4,nb_imp_az);
      len=0; az_motor_rq=0;  el_motor_rq=0;
      enable_interrupts(INT_RDA);
      lcd_gotoxy(16,2);
      track='*';
      lcd_putc(track);
      }
	affiche_position(2, position_site , position_azimut/2);//(cible_azimut/2)%360)
   }
}