/*
	CONTROL DE ACTUADORES Y SENSORES MEDIANTE PUERTO PARALELO

	Control de un servomotor.
	Alimentaci¢n del servo conectada al primer pin de datos mediante una
	etapa de potencia (transistores TIP125-120).
	L¡nea de control del servo conectada directamente al segundo pin de datos
													 +
									D0 --- (etapa) ---- SERVO ------ D1
														  |
									GND	------------------|
													 -
	2002 V¡ctor R. Gonz lez
*/

#include <stdio.h>
#include <dos.h>

/* Direcciones del puerto paralelo */
#define LPT_BASE	0x378
#define DATOS		LPT_BASE
#define ESTADO		LPT_BASE+1
#define CONTROL		LPT_BASE+2

/* Posiciones angulares del servo (Hitec HS-300) */
#define ANG_MIN         0.		/* posici¢n angular m¡nima (§) */
#define ANG_NEUTRO      90.		/* posici¢n angular neutra (§) */
#define ANG_MAX			180.	/* posici¢n angular m xima (§) */

#define PULSO_MIN		500.	/* tiempo (us) pulso de control para   0§ */
#define PULSO_NEUTRO	1500.	/* tiempo (us) pulso de control para  90§ */
#define PULSO_MAX       2500.	/* tiempo (us) pulso de control para 180§ */
#define PULSO_OFF		20000.	/* tiempo (us) pulso de control en OFF */

#define APAGA_SERVO		0x01	/* D0 alto (pin 2) alimentaci¢n del servo */
#define ENCIENDE_SERVO	0x00	/* D0 bajo (pin 2) alimentaci¢n del servo */
#define CONTROL_SERVO	0x02	/* D1 alto (pin 3) control del servo */

/* Para retardos de precisi¢n */
#define us_CUENTA		15.086	/* n§ de microsegundos por cuenta */

/* Macro que env¡a un valor al puerto de DATOS */
#define Activa(valor)	outportb (DATOS, valor)

/* Env¡a pulsos PWM al servo */
void servo_pulsos	(float angulo, unsigned num_pulsos);
/* Retardo de precisi¢n */
void pausa			(unsigned long);


main ()
{
	float angulo;
	unsigned num_pulsos;

	printf("Control de un servomotor:\n");

	Activa (ENCIENDE_SERVO);	/* Alimenta el servo */

	do {
		printf ("\tIntroduce la posici¢n angular final del servo: ");
		scanf  ("%f", &angulo);
		printf ("\tIntroduce el n£mero de pulsos de control (0=salir): ");
		scanf  ("%u", &num_pulsos);

		if (num_pulsos >0)
			servo_pulsos (angulo, num_pulsos);

	} while (num_pulsos > 0);

	Activa (APAGA_SERVO);	/* Apaga la alimentaci¢n del servo */

	return 0;

}


/* Posiciona un servomotor en el  ngulo requerido
*/
void servo_pulsos (float angulo, unsigned num_pulsos)
{
	int pulso;
	float tiempo_pulso;
	unsigned long cuentas_pulso, cuentas_off;

	/* Duraci¢n del pulso */
	tiempo_pulso = PULSO_MIN +
					angulo * (PULSO_MAX-PULSO_MIN) / (ANG_MAX-ANG_MIN);
	cuentas_pulso = tiempo_pulso / us_CUENTA;

	printf ("\t\t(Duraci¢n del pulso: %6.1f us, %u cuentas)\n",
			tiempo_pulso, cuentas_pulso);

	/* Duraci¢n (en cuentas) de pulso apagado */
	cuentas_off = PULSO_OFF/us_CUENTA;

	for (pulso=1; pulso<num_pulsos; pulso++) {

		/* Activa la l¡nea de control */
		Activa (CONTROL_SERVO | ENCIENDE_SERVO);	pausa (cuentas_pulso);

		/* Desactiva la l¡nea de control */
		Activa (ENCIENDE_SERVO);					pausa (cuentas_off);
	}

	Activa (APAGA_SERVO);	/* Apaga la alimentaci¢n del servo */
}


/*	Retardo de precisi¢n

	El tiempo se mide en cuentas.
	Un segundo equivale a 66288 cuentas.
	Una cuenta equivale a 1/66287.78 s = 15.086 microsegundos.
	Esta es la resoluci¢n temporal que se consigue con este m‚todo.
*/
void pausa (unsigned long cuenta)
{
	register a = NULL, b;
	unsigned cuenta_h, cuenta_l;

	cuenta_h=cuenta >> 16;  cuenta_l=cuenta & 0xFFFF;

	do
		do {
			while (a==(b=inportb(0x61) & 0x10));
			a=b;
		} while (cuenta_l--);
	while (cuenta_h--);
}