#include <track_ana_imp.h>
#include <stdlib.h>

#define use_portb_lcd TRUE

#include <LCD.C>

#define LEFT (PIN_C4)     // signal de commande left
#define RIGHT (PIN_A5)    // signal de commande right
#define DOWN (PIN_A2)     // signal de commande down
#define UP (PIN_C3)       // signal de commande up
#define CMD_LEFT (PIN_A4) // Poussoir de commande left
#define CMD_RIGHT (PIN_C5)// Poussoir de commande right
#define CMD_DOWN (PIN_C1) // Poussoir de commande down
#define CMD_UP (PIN_C2)     // Poussoir de commande up
#define AZ (PIN_A1)       // signal analogique d'azimut
#define PULSE (PIN_C0)    // impulsions du codeur de site

#define BUTEE_SUD TRUE  // commenter la ligne si butée nord

#priority TIMER1,RDA

// ******************** variables globales ********************************//
int impulsion=2;
static int16 nb_impulsion;         // nombre d'impulsions
static int16 cible_site;          // valeur site demandée
static int32 cible_azimut;        // valeur azimut demandée
static int16 position_site;          // position site actuelle
static int32 position_azimut;        // position azimut actuelle
static int32 visu_position_azimut;
static int32 visu_cible_azimut;
static int up_on;
static int down_on;
static int left_on;
static int right_on;
static int16 table_site[19];
static char rcv[60];
int a=0,b=0;
static int16 cible_site_impulsion;
static int el_motor_rq=0;
static int az_motor_rq=0;
static int len=0,ref_el=100;
static int8 last_C=0;
static int8 tempo=0;
static int32 depart_az;

#byte port_c=7

#int_TIMER1
TIMER1_isr()
{
tempo++;
}
#int_TIMER2
TIMER2_isr()
// Routine d'interruption sur timer overflow toute les 13ms
// pour scruter chaque changement sur le port C
// Compte ou décompte les changements d'états de l'entrées C0
// selon le sens de rotation du vérin .
// Deux changements d'état par impulsion.
{
int8  change;

// lecture du port B et isolement des deux bits B4 et B5
change = (port_c & 0b00000001) ^ last_c; // Identification des changements
last_c = port_c;  // Rafraichissement de la position du port

if( bit_test (change, 0))
   {
   if(down_on==1)
      nb_impulsion-=1;
   if(up_on==1)
      nb_impulsion+=1;
   }
}




//////////////////////////////
// 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);
}

void lecture_az (void)
// Procédure de lecture du canal analogique d'azimut qui renseigne la variable globale
// position_azimut.
{
   setup_adc_ports(RA0_RA1_RA3_ANALOG);
   setup_adc(ADC_CLOCK_INTERNAL);
   set_adc_channel(1);
   delay_us(100);
	position_azimut = read_adc();
   position_azimut= ((position_azimut*360)/920);
   delay_ms(5);
   setup_adc( ADC_OFF );
#if defined BUTEE_SUD
   visu_position_azimut=(position_azimut+180)%360; // si butée sud
#else
   visu_position_azimut=position_azimut; //si butée nord
#endif
}


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;

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


int16 GetNbDegre(Int16 iImpulsion)
//********************* VERIN DE SITE *****************************************************
// Procédure qui donne la correspondance en degrés de la valeur du compteur d'impulsions.
// 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;
}

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 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 cal_el(void)
{
	int a;
   int16 c;
   disable_interrupts(global);
   lcd_gotoxy(1,1);
   lcd_putc("SET ELEV TO ZERO");
   lcd_gotoxy(1,2);
   lcd_putc("Push 'RIGHT' when Ok ");
	while (input(CMD_RIGHT)==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,100);
   delay_ms(1000);
   reset_cpu();
}

#SEPARATE
void config_el (void)
{
	int a;
   int16 c;
   disable_interrupts(global);
   nb_impulsion=ref_el;
   lcd_gotoxy(1,1);
   lcd_putc("CONFIG ELEVATION TBL");
   lcd_gotoxy(1,2);
   lcd_putc("Push 'RIGHT' when Ok");
	while (input(RIGHT)==1)
         a=0;
   delay_ms(1000);
   lcd_gotoxy(1,1);
   lcd_putc("Push 'RIGHT' when Ok");
   enable_interrupts(global);
   for (a=20,c=0;c<=90;a=a+2,c=c+5)
		{
		while (input(CMD_RIGHT)==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_impulsion);
			 }
      disable_interrupts(global);
      delay_ms(500);
		ecrire_eeprom(a,nb_impulsion);
		}
      ecrire_eeprom(2,nb_impulsion);
      write_eeprom(6,55);
		delay_ms(1000);
reset_cpu();
}



#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;
   disable_interrupts(global);
   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("READ EL TBL /Right");
       lcd_gotoxy(1,2);
       lcd_putc("EL");
       format_affiche (c);
       lcd_putc(0xdf);
       lcd_putc(" IMP ");
       format_affiche(imp_el);
       while (input(CMD_RIGHT)==1)
          {
          delay_ms(1);
          }
        delay_ms(800);
      }
}



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);
   visu_cible_azimut=atol(szValue1);

#if defined BUTEE_SUD
   cible_azimut=(visu_cible_azimut+180)%360; // si  butée sud
#else
   cible_azimut=visu_cible_azimut;
#endif

	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];
char string3[4];
char string4[5];

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

   if ((strstr(rcv,string1) && strstr(rcv,string2) && (strstr(rcv,string3) ||
               strstr(rcv,string4))) || (rcv[0]=='M' && len==5) || (rcv[0]=='W' && len==9) )
      {
      if (extract())
         {
         lcd_gotoxy(16,1);
         lcd_putc("T");
	      affiche_position (1, cible_site , visu_cible_azimut);
         lecture_az();
         if (cible_azimut > (position_azimut+2) || (cible_azimut+10) < ((position_azimut+10)-2) )      // 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_impulsion);
         if (cible_site > (position_site+2) || (cible_site+10) < ((position_site+10)-2) )     // Si changement de position détectée > 2°
               el_motor_rq=1;    // Demande de démarrage moteur de site
         else
               el_motor_rq=0;
         }
      }
   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_impulsion);
cible_site_impulsion=GetNbImpulsion(cible_site);
if (down_on && (nb_impulsion <= cible_site_impulsion)) // Arret en descente
      {
      output_low(DOWN);
      affiche_position(2, position_site , visu_position_azimut);
      el_motor_rq=2;
		delay_ms(1500);
      down_on=0;
		return;
      }

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

if  (nb_impulsion > cible_site_impulsion)
      {
      if (up_on)
         {
         output_low(UP);
    		delay_ms(1500);
         up_on=0;
         }
      down_on=1;
      output_high(DOWN);
	   return;
	   }
if  (nb_impulsion < cible_site_impulsion)
      {
      if (down_on)
         {
         output_low(DOWN);
   		delay_ms(1500);
         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)

{
int16 ecart_depart,ecart_arret;

lecture_az(); // lecture de la position actuelle du moteur
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 ( right_on && (position_azimut >= cible_azimut)) // Arret en marche avant
      {
      output_low(RIGHT);
     	affiche_position(2, position_site , visu_position_azimut);
		delay_ms(1500);
      az_motor_rq=2;
      right_on=0;
		return;
      }
if ( left_on && (position_azimut <= cible_azimut)) // Arret en marche arrière
      {
      output_low(LEFT);
      affiche_position(2, position_site , visu_position_azimut);
		delay_ms(1500);
      az_motor_rq=2;
      left_on=0;
		return;
      }
if  (position_azimut > cible_azimut)
      {

      if (right_on)
         {
         output_low(RIGHT);
    		delay_ms(1500);
     	   right_on=0;
         }
      left_on=1;
      output_high(LEFT);
	  if (ecart_arret < 8 || ecart_depart < 8)
		{
      output_bit(left,0);
		delay_ms(5);
      output_bit(left,1);
      }

		return;
	   }
   if  (position_azimut < cible_azimut)
	   {
      if (left_on)
         {
         output_low(LEFT);
   		delay_ms(1500);
         left_on=0;
         }
      right_on=1;
      output_high(RIGHT);
	  if (ecart_arret < 8 || ecart_depart < 8 )
		{
      output_bit(right,0);
		delay_ms(5);
      output_bit(right,1);
      }

      return;
	   }
}

#SEPARATE

int 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 (input(CMD_UP)==0 || input(CMD_DOWN)==0 || input(CMD_LEFT)==0 || input(CMD_RIGHT)==0)

{
if (input(CMD_UP)==0 || input(CMD_DOWN)==0)
   {
   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_impulsion); position_site=cible_site;
   }
if (input(CMD_LEFT)==0 || input(CMD_RIGHT)==0)
   {
   output_low(LEFT);
   output_low(RIGHT);
   delay_ms(1000);
   az_motor_rq=2;
   right_on=0; left_on=0;
   lecture_az();
   cible_azimut = position_azimut;
   visu_cible_azimut=visu_position_azimut;
   }
 	affiche_position(2, position_site , visu_position_azimut);
  	affiche_position(1, cible_site , visu_cible_azimut);

   enable_interrupts(INT_TIMER1);
   tempo=0;
   
   while (tempo<12)
      {
      int j=1;
      int i=1000;
  
      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, visu_cible_azimut);
         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, visu_cible_azimut);         
         tempo=0;
         }
      while ((input(CMD_RIGHT)==0) && (input(CMD_LEFT)==1 ))
         {
         if (cible_azimut < 360 )
          	cible_azimut+=1;
  		   delay_ms(i);
         j=j++;
  		   if (i>=20){ i=i-(10*j*j);}
#if defined BUTEE_SUD
   visu_cible_azimut=(cible_azimut+180)%360; // si  butée sud
#else
   visu_cible_azimut=cible_azimut;  // si butée nord
#endif
	     	affiche_position(1, cible_site, visu_cible_azimut);
         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);}
#if defined BUTEE_SUD
   visu_cible_azimut=(cible_azimut+180)%360; // si  butée sud
#else
   visu_cible_azimut=cible_azimut;  // si butée nord
#endif
	     	affiche_position(1, cible_site, visu_cible_azimut);
         tempo=0;
        }
      }
   disable_interrupts(INT_TIMER1);
   lcd_gotoxy(16,1);
   lcd_putc("M");

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

       lecture_az();
       if (cible_azimut > (position_azimut) || cible_azimut < (position_azimut) )      // 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;
   }
}

void main()
{
   setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);
   setup_timer_2(T2_DIV_BY_16,197,16); //10ms
   disable_interrupts(INT_TIMER1); // 104ms

   enable_interrupts(INT_TIMER2);
   setup_adc(ADC_OFF);
   enable_interrupts(INT_RDA);
   enable_interrupts(global);

   SET_TRIS_A(0x13);          // Port A avec A2,A4,A5 en sorties; A0,A1,A3 en entrées
   SET_TRIS_B(0x00);          // Port B en sorties
   SET_TRIS_C(0xE7);          // Port C
   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

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

// Configuration du controleur

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

   if (input(CMD_UP)==0)
      lecture_table();
	if (input(CMD_DOWN)==0)
		config_el();
   if (input(cmd_left)==0)
      cal_el();


   nb_impulsion=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_impulsion);
   cible_site=position_site;

   lecture_az();
   cible_azimut=position_azimut;
   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)
   {
	trt_rs232();     // Demande de positionnement ou de tracking via la COM ?
   req_pos_ant();
   lecture_az(); // lecture de la position actuelle du moteur d'azimut

   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(0x02,nb_impulsion);    // sauvegarde du nombre d'impulsions
      len=0; az_motor_rq=0;  el_motor_rq=0;
      enable_interrupts(INT_RDA);
      lcd_gotoxy(16,2);
      lcd_putc("*");
      }
	affiche_position(2, position_site , visu_position_azimut);
   }
}