/* Program control orientare iluminat  functie de pozitie volan.
Acesta  are un potentiometru a carui cursor este 
solidar cu axul volanului. Orientarea farurilor este realizata
cu un servo. 
Tensiune de pe cursor este aplicata la canalul 4 al convertorului
analo numeric. Un schelet de multitasching este utilizat pentru
partajarea timpului de achizitie cu cel de control servo. 
Conexiuni:
    AN4  - volan fir rosu
    GND  - volan fir galben
    3,3V - volan fir negru 
    RD8  - ServoX 
    RD9  - ServoY care actioneaza far (LED)
    RB5  - ServoZ
Culor servo: maro : GND
             rosu : +5V
           galben : data
*/
 
#include <p33FJ256GP506.h>  

 
#define x       _LATD8 // comanda servoX - far
#define y       _LATD9 // comanda servoY
#define z       _LATB5 // comanda servoZ
        
_FGS(GWRP_OFF & GCP_OFF);
_FOSCSEL(FNOSC_FRC);
_FOSC(FCKSM_CSECMD & OSCIOFNC_ON & POSCMD_NONE);
_FWDT(FWDTEN_OFF);

int i,j,d;
int  alfax;       // unghi rotatie servo x
                  
int  alfay;       // unghi rotatie  servo iluminat
int  alfaz;       // unghi rotatie z
int Countxa = 0;  // durata comanda x in 1
int Countxb = 0;  // durata comanda x in 0
int Countya = 0;  // durata comanda y in 1
int Countyb = 0;  //  durata comanda y in 0
int Countza = 0;  //  durata comanda z in 1
int Countzb = 0;  //  durata comanda z in 0

void initadc(void)    // initializeza modul AD canal 4 pt poz. volan
{
           _TRISB4=1;              // AN4 este conectat la RB4 pin 12 
          ADPCFG=0xFFEF ;          // activare canal 4
          AD1CON1bits.AD12B  = 1;  // Convertor pe 12 biti
          AD1CON1bits.FORM   = 0;  //  Intreg aliniat dreapta
          AD1CON1bits.SSRC   = 0;  // conversie contraolata cu SAMP
          AD1CON1bits.SIMSAM = 0;
          AD1CON1bits.ASAM   = 0;
          AD1CHS0=0x04;            // selectie canal 4  
          AD1CON3=0x0005;          //  T conversie este 5*Tcy  
          AD1CON2=0 ;              // fara scanare , 
          IFS0bits.AD1IF = 0;	   // Clear the A/D int flag
          IEC0bits.AD1IE = 0;	   // Desactivare int
          AD1CON1bits.ADON = 1;    // Activare convertor A/D

}
   
int readadc()                       // returneaza valoare ADC1BUF0 
{
int val;
      AD1CON1bits.SAMP=1;           // start  sample
          for(j=0;j<2;j++);         //delay

      AD1CON1bits.SAMP=0;            // start conversie 
          while(!AD1CON1bits.DONE);  // astept sfirsit conversie
          val=ADC1BUF0; 
          IFS0bits.AD1IF = 0;        // Readuce contor buffer la zero
        return (val);
} 




void Init_Timer1( void )
{

    T1CON = 0;              // TCKPS=0, TCS=0, TGATE=0
    // clok intern(FRC) 7,37tMhz, perioda de  135,6 nanosecunde
    // clock timer1 Fcy=2*135,6=271,2 nanosecunde
 	IFS0bits.T1IF = 0;      // Reset Timer1 interrupt flag
	 
 	TMR1=  0x0000;          // valoare initiala in contor	
	PR1 = 0x001A;           // Valoare comparator, intrerupere 
                            //la  25 microsecunde
	
    IPC0bits.T1IP = 6;      // Timer1 int. priority level=4   
	T1CONbits.TON = 1;      // Pornire timer1
	IEC0bits.T1IE = 1;      // Activare  interrupere timer1

}

void __attribute__((interrupt,no_auto_psv)) _T1Interrupt( void )
    {     	// survine la  25 microsecunde
	
     // IEC0bits.T1IE = 0; // desactibez validare timer 1
    	IFS0bits.T1IF = 0; //desactivez intrerupere timer
    	T1CONbits.TON = 0; // opresc Timer1

    	
   	Countxa++;
	if(Countxa == alfax) // 0 grade
  	{                             
		Countxa = 0;  
	
        x=0;  // opresc comnda	x
    } 	    
  
     Countxb++;
     if(Countxb==1000)  // 1000 durata de refresh de 25ms
       {		
          Countxb=0;
		  Countxa = 0;  
         
          x=1;  // pornesc comanda x
       }
// Control pozitie servo axa Y

    Countya++;
	if(Countya == alfay)  // 90 grade
  	{                       
	 
        y=0;  // opresc comnda	y
    } 	    
     Countyb++;
        if(Countyb==1000)
       {
          Countyb=0;
          	Countya = 0; 
          y=1;  // pornesc comanda x
       }
       
       
// Control pozitie axa Z  
 	Countza++;
	if(Countza == alfaz) // 180 grade
  	{                       
		Countza = 0;  
	
        z=0;  // opresc comnda	z
    } 	    
     Countzb++;
        if(Countzb==1000)
       {
          Countzb=0;
          Countza = 0; 
         
          z=1;  // pornesc comanda z
       }

   	
    	TMR1  = 0; //reincarc timer
	T1CONbits.TON = 1; // pornesc timer

}

void delay()
{
	for(i=0;i<1000;i=i+1)  // delay aprox 27 milisecunde pt i=100
	         j=0;
}

void main (void)

{
	    _TRISD8=0;  //  sens servoX
	    _TRISD9=0;  //  sens servoY
        _TRISB5=0;  //  sens servoZ
    

 
         x=0; // servoX
         y=0; // servoY
         z=0; // servoZ
       
	initadc();
	Init_Timer1();

    for (;;)

	{

	// Lectura pozitie volan si control poz. far
        alfay=60 +readadc()/150;  
/* readadc() returneaza val 153 la max sting, 2034 la mijloc
   si 3910 la max. dreapta */
        alfax= 70;           // comenzi ulterioare

        alfaz= 80;           // comenzi ulterioare

	}
}