/**************************************************************************
*                                                                         *
*                        -TALLER DE ELECTRÓNICA-                          *
*                              OTTO KRAUSE                    	      	  *
*						                                                              *
* NOMBRE DEL PROGRAMA:                                                    *
*                                                                         *
* ALUMNO:                                                                 *
*                                                                         *
* CURSO Y DIVISIÓN:                                                       *
*                                                                         *
**************************************************************************/

/**************************************************************************
*                        Inclusión de bibliotecas                         *
**************************************************************************/

#include <hidef.h> 
#include "derivative.h" 
#include <stdlib.h>

/**************************************************************************
*                        Definición de constantes                         *
**************************************************************************/
#define paso1 PTBD_PTBD7
#define paso1_DD PTBDD_PTBDD7
#define paso1_PE PTBPE_PTBPE7

#define dir1 PTBD_PTBD6
#define dir1_DD PTBDD_PTBDD6
#define dir1_PE PTBPE_PTBPE6

#define paso3 PTBD_PTBD5
#define paso3_DD PTBDD_PTBDD5
#define paso3_PE PTBPE_PTBPE5

#define dir3 PTBD_PTBD4
#define dir3_DD PTBDD_PTBDD4
#define dir3_PE PTBPE_PTBPE4

#define paso2 PTBD_PTBD3
#define paso2_DD PTBDD_PTBDD3
#define paso2_PE PTBPE_PTBPE3

#define dir2 PTBD_PTBD2
#define dir2_DD PTBDD_PTBDD2
#define dir2_PE PTBPE_PTBPE2

#define sensor1 PTCD_PTCD0
#define sensor1_DD PTCDD_PTCDD0
#define sensor1_PE PTCPE_PTCPE0

#define sensor2 PTCD_PTCD1
#define sensor2_DD PTCDD_PTCDD1
#define sensor2_PE PTCPE_PTCPE1

#define sensor3 PTCD_PTCD2
#define sensor3_DD PTCDD_PTCDD2
#define sensor3_PE PTCPE_PTCPE2

#define sensor4 PTCD_PTCD3
#define sensor4_DD PTCDD_PTCDD3
#define sensor4_PE PTCPE_PTCPE3

#define sensor5 PTAD_PTAD3
#define sensor5_DD PTADD_PTADD3
#define sensor5_PE PTAPE_PTAPE3

#define sensor6 PTAD_PTAD5
#define sensor6_DD PTADD_PTADD5
#define sensor6_PE PTAPE_PTAPE5

#define AD0 0b00100000;
#define AD1 0b00100001;
#define AD2 0b00100010;
//#define AD3 0b00100011;


/**************************************************************************
*                        Declaración de funciones                         *
**************************************************************************/

void retardo(int);
void AD(void);
void Inicio1(void);
void Inicio2(void);
void Inicio3(void);
void motor(long,long,long);
int D,pote1,pote2,pote3,rad1=0,rad2=0,rad3=0;
unsigned long m1c=0,m2c=0,m3c=0,m1d=0,m2d=0,m3d=0;

int m1r=0,m2r=0,m3r=0;
int s1on=0;
int s2on=1;
int s3on=1;
int s4on=0;
int s5on=1;
int s6on=0;


void main(void)
{

  /************************************************************************
  *                        Declaración de variables                       *
  ************************************************************************/
  
  
  
  /************************************************************************
  *                       Inicializacón de variables                      *
  ************************************************************************/
  
  
  
  /************************************************************************
  *                     Habilitación de interrupciones                    *
  ************************************************************************/
 
   EnableInterrupts; 
 
  /************************************************************************
  *                    Configuración del microcontrolador                 *
  ************************************************************************/
  
 
  
  /************************************************************************
  *                          Configuración puertos                        *
  ************************************************************************/
  
   SOPT1 = 0x02; /* deshabilito COP */
   
   ADCCFG=0b01000000; /* fadc clock: fbus / 4 = 2MHz */
   ADCSC2=0;
   ADCSC1=0b00100010; /* Entrada para ADC: AD2 (pata 18 del SH8) */
   APCTL1=0b00001111; /* Entrada para ADC: AD2 (pata 18 del SH8) */
   
   PTADD=0x00; // Configuro puertos como entrada/salida.
   PTBDD=0xff;
   PTCDD=0x00;
   
   PTAPE=0xff; // Habilito/deshabilito Pull ups de puertos.
   PTBPE=0x00;
   PTCPE=0x0f;    

   PTAD=0x00; // Limpio puertos.
   PTBD=0x00;
   PTCD=0x00;
  
  /************************************************************************
  *                                Programa                               *
  ************************************************************************/
   retardo(3);
  	//.Inicio1();
  	 retardo(3000);
	//Inicio2();
	 retardo(3000);
	Inicio3();
	 retardo(300);
	
	while(1)
	{
		m1d=0;
		m2d=0;
		m3d=0;
		AD();
		m1d=pote1*rad1;
		m2d=pote2*rad2;
		m3d=pote3*rad3;
     retardo(3);
    
     if(sensor2==s1on)
     {
    	 m1c=0;
     }
     if(sensor1==s1on)
          {
         	 m1c=(rad1*255);
          }
     if(sensor3==s3on)
        {
    	 m2c=(rad2*255);
        }
        if(sensor4==s4on)
             {
        	m2c=0;
             }
        if(sensor5==s5on)
                {
            	 m3c=(rad3*255);
                }
                if(sensor6==s6on)
                     {
                	m3c=0;
                     }

		if(m1c<m1d)
		{
		  m1r++;
		  m1c++;
		  dir1=0;
		}
		
		if(m1c>m1d)
		{  
		  m1r--;
		 m1c--;
		 dir1=1;
		}
		if(m2c<m2d) 
		{  
		  m2r++;
		  m2c++;
		  dir2=0;
		}
		
		if(m2c>m2d) 
		{   
		  m2r--;
		  m2c--;
		  dir2=1;
		}if(m3c<m3d) 
		{  
		  m3r++;
		  m3c++;
		  dir3=0;
		}
		
		if(m3c>m3d) 
		{   
		  m3r--;
		  m3c--;
		  dir3=1;
		}

	if(m1r>1)//control de variables de los motores
	m1r=0;
	
	if(m1r<0)
	m1r=1;
	
	if(m2r>1)
	m2r=0;
	
	if(m2r<0)//control de variables de los motores
	m2r=1;
	
	if(m3r>1)//control de variables de los motores
	m3r=0;
	
	if(m3r<0)
	m3r=1;
	
	switch(m1r) // motor 1
			{
				case 0: paso1=0;
						break;
				
				case 1: paso1=1;
						break;
			}
	switch(m2r) // motor 2
		{
				case 0:	paso2=0;
						break;
				
				case 1: paso2=1;
						break;
		}
	switch(m3r) // motor 3
			{
				case 0:
						paso3=0;break;
				
				case 1: 
						paso3=1;break;
			}
	}
}

/**************************************************************************
*                        Prototipo de funciones                           *
**************************************************************************/
	void Inicio1(void)
	{	static int e1=0;
		static unsigned long cont1=0;
		sensor1_PE =1; 
		sensor2_PE =1; 
		while(e1!=3)
		{
			if(e1==0)
				
				
			{
				
				
				
				dir1=0;
				paso1=0;
				retardo(15);
				paso1=1;
				retardo(15);
				if(sensor1==s1on)
					e1=1;
				
			}
			if(e1==1)
			{
				cont1++;
				dir1=1;
				paso1=0;
				retardo(15);
				paso1=1;
				retardo(15);
				cont1++;
				if(sensor2==s2on)
					e1=2;
				
			}
			if(e1==2)
			{
				m1c=0;
				rad1=cont1/255;
				e1=3;
			}
		}
		
		
	}
			
	void Inicio2(void)
	{	static int e2=0;
		static unsigned long cont2=0;
		sensor3_PE =1; 
		sensor4_PE =1; 
		while(e2!=3)
		{
			if(e2==0)
				
				
			{
				
				
				
				dir2=0;
				paso2=0;
				retardo(15);
				paso2=1;
				retardo(15);
				if(sensor3==s3on)
					e2=1;
				
			}
			if(e2==1)
			{
				cont2++;
				dir2=1;
				paso2=0;
				retardo(15);
				paso2=1;
				retardo(15);
				cont2++;
				if(sensor4==s4on)
					e2=2;
				
			}
			if(e2==2)
			{
				m2c=0;
				rad2=cont2/255;
				e2=3;
			}
		}
		
		
	}	
	void Inicio3(void)
					{	static int e3=0;
						static long cont3=0;
						sensor5_PE =1; 
						sensor6_PE =1; 
						while(e3!=3)
						{
							if(e3==0)
							{
								dir3=0;
								paso3=0;
								retardo(15);
								paso3=1;
								retardo(15);
								if(sensor5==s5on)
									e3=1;
								
							}
							if(e3==1)
							{
								cont3++;
								dir3=1;
								paso3=0;
								retardo(15);
								paso3=1;
								retardo(15);
								if(sensor6==s6on)
									e3=2;
								
							}
							if(e3==2)
							{
								m3c=0;
								rad3=cont3/255;
								e3=3;
							}
						}
						
						
					}
	
	void AD(void){
		
		static int e=0;
		static int n_ad;
		
		if(e==0){
			

			ADCCFG=0b01000000; // fadc clock: fbus / 4 = 2MHz 
			ADCSC2=0;
			ADCSC1= AD0; // Entrada para ADC: AD2 (pata 18 del SH8) 
			APCTL1=0b00001111; // Entrada para ADC: AD2 (pata 18 del SH8) 
			
			
			
			
			e=1;
			n_ad=0;
		}
		else if(e==1 && ADCSC1_COCO==0){
			
		}
		else if(e==1 && ADCSC1_COCO==1 && n_ad==0){
			e=2;
			pote1 = ADCRL;
			ADCSC1=AD1; // Entrada para ADC: AD1 
			//APCTL1=0b00000010;
			n_ad=1;
		}
		else if(e==2){
			e=1;
		}
		else if(e==1 && ADCSC1_COCO==1 && n_ad==1){
			e=3;
			pote2 = ADCRL;
			ADCSC1=AD2; // Entrada para ADC: AD2 
			//APCTL1=0b00000100;
			n_ad=2;
		}
		else if(e==3){
			e=1;
		}
		else if(e==1 && ADCSC1_COCO==1 && n_ad==2){
			e=4;
			pote3 = ADCRL;
			ADCSC1=AD0; // Entrada para ADC: AD0
			//APCTL1=0b00000001;
			n_ad=0;
		}
		else if(e==4){
			e=1;
		}
		
	}

void retardo(int papa)
{
  int i=1,j=1;

  for(i=1;i<=papa;i++)
    for(j=1;j<=1;j++);
}