cam_control_fw.c

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY without ev

						CLEAR_HSNAK();
					} else {
						EP0_STALL();
					}
					break;

				case SET_CONFIG_REQUEST:
					/* only one configuration is supported */
					if (sdata->bmRequestType == 0 && sdata->wValueL == 0x01) {
						CLEAR_HSNAK();
					} else {
						EP0_STALL();
					}
					break;

				case GET_INTERFACE_REQUEST:
					/* only one interface */
					if(sdata->bmRequestType == 0x81 &&
						sdata->wIndexL == 0x00 &&
						sdata->wLengthL == 0x01) {
 
						in0buf[0] = 0;
						IN0BC = 0x01;
						CLEAR_HSNAK();
					} else {
						EP0_STALL();
					}
					break;

				case SET_INTERFACE_REQUEST:
					set_interface();
					break;

				default:
					EP0_STALL();
					break;
			}
		} else {
			EP0_STALL();
		}
	} else if (IN07IRQ & 0x01) {
		/* IN 0 interrupt */
		IN07IRQ = 0x01;
	} else if (OUT07IRQ & 0x01) {
		/* OUT 0 interrupt */
		OUT0BC = 0;
		OUT07IRQ = 0x01;  
	} else if (OUT07IRQ & 0x02) {
		/* OUT 1 interrupt */
		endpoint1_out();
		OUT1BC = 0;
		OUT07IRQ = 0x02;
	} else if (IN07IRQ & 0x02) {
		/* IN 1 interrupt */
		IN07IRQ = 0x02;
		if (ep1in_pending) {
			ep1in_pending = 0;
			send_cc_info();
		}
	} 

	/* reset - don't reset, servers may go crazy */
	if (USBIRQ & 0x10){
	 USBIRQ = 0x10;
	}

}


/****************************************************
 * reset_ezusb
 ****************************************************/
static void 
reset_ezusb(void)
{

	EA = 0;          /* disable global interrupts */

	USBCS &= 0xF3;   /* Clear DISCON, DISCOE */
	wait_msec(200);  /* give host plenty of time to respond to disconnect */

	USBBAV = 0;      /* clear autovectoring and break reporting         */
	USBIRQ = 0xFF;   /* clear SUDAV interrupt and all other interrupts  */
	USBIEN = 0x11;   /* enable SUDAV interrupt and RESET interrupt      */

	IN07IRQ = 0xFF;  /* clear all IN interrupts       */
	IN07IEN = 0x03;  /* enable IN0 & IN1 interrupts   */
	IN07VAL = 0x03;  /* make endpoint IN0 & IN1 valid */

	OUT07IRQ = 0xFF; /* clear all OUT interrupts         */
	OUT07IEN = 0x03; /* enable OUT0 & OUT1 interrupts    */
	OUT07VAL = 0x03; /* make endpoint OUT0 & OUT1  valid */
  
	USBCS |= 0x06;   /* Set DISCOE to enable pullup resistor 
							* Set RENUM so that 8051 handles USB requests
							*/

	PORTACFG &= ~0x80;   /* port A7 has LED attached for blinking */
	OEA |= 0x80;
	OUTA &= ~0x80;       /* initial state of LED is off */

	PORTCCFG &= ~0x08;   /* on DeVaSys board blink port C3 */
	OEC |= 0x08;
	OUTC |= ~0xF7;       /* initial state of LED is off (led is tied to +v) */
	                     /* so bring port high to turn it off               */
	OUTC &= 0xF7;       /* ON */

	PORTBCFG &= ~0x0C;   /* port B2 is horizontal servo & B3 is vertical */
	OEB |= 0x0C;

	EUSB = 1;        /* enable USB interrupt     */
	EA = 1;          /* enable global interrupts */

}


/****************************************************
 * set up the timers to control the servos
 ****************************************************/
static void 
init_timers(void) critical
{
	unsigned short usecs;

	timer1_action = STOP_BOTH_PULSES;
	timer1_usecs_remaining = 0;

	/* configure initial pulse widths for each timer */
	usecs = SERVO_INTERRUPT_TIME(pulse_frequency);
	TH0 = HI_BYTE(usecs);
	TL0 = LOW_BYTE(usecs);
	usecs = SERVO_INTERRUPT_TIME(center_usecs);
	TH1 = HI_BYTE(usecs);
	TL1 = LOW_BYTE(usecs);

	/* configure timer0 and timer1 */
	TMOD = 0x11;
	TCON |= 0x50;

	/* allow interrupts from timers */
	ET0 = 1;
	ET1 = 1;

	/* start servo pulses */
	HORIZONTAL_SERVO_ON();
	VERTICAL_SERVO_ON();

}


/****************************************************
 * interrupt runs every 20 milli-seconds, starts
 * a new pulse on each servo, and re-starts pulse 
 * duration timers
 ****************************************************/
static void 
timer0_isr(void) interrupt 1 using 2 critical
{

	/* stop the timers */
	TCON &= ~0x50;

	if (pwm_message.new_command) {
		/* new horizontal or vertical postion has been given */
		final_horizontal_pos = pwm_message.horizontal_pos;
		final_vertical_pos = pwm_message.vertical_pos;
		pwm_message.new_command = 0;
	}

#if DELAY_PER_PERIOD
   if (servo_delay_count++ >= DELAY_PER_PERIOD)
#endif /* DELAY_PER_PERIOD */
	{

		if (current_horizontal_pos < final_horizontal_pos) {
			current_horizontal_pos += 1;
		} else if (current_horizontal_pos > final_horizontal_pos) {
			current_horizontal_pos -= 1;
		}

		if (current_vertical_pos < final_vertical_pos) {
			current_vertical_pos += 1;
		} else if (current_vertical_pos > final_vertical_pos) {
			current_vertical_pos -= 1;
		}
#if DELAY_PER_PERIOD
		servo_delay_count = 0;
#endif /* DELAY_PER_PERIOD */
	} 

	/* set timer0 to pulse interval */
	usecs = SERVO_INTERRUPT_TIME(pulse_frequency);
	TH0 = HI_BYTE(usecs);
	TL0 = LOW_BYTE(usecs);

	/* determine which pulse ends first and set the timer to expire
	 * at that time, the timer will be started again for the remaining
	 * portion of the longer timer period
	 */
	if (current_vertical_pos < current_horizontal_pos)
	{
		POSITION_TO_USEC(current_vertical_pos, usecs);
		POSITION_TO_USEC(current_horizontal_pos, timer1_usecs_remaining);
		timer1_action = HORIZONTAL_TIME_REMAINING;
	}
	else if (current_horizontal_pos < current_vertical_pos)
	{
		POSITION_TO_USEC(current_horizontal_pos, usecs);
		POSITION_TO_USEC(current_vertical_pos, timer1_usecs_remaining);
		timer1_action = VERTICAL_TIME_REMAINING;
	}
	else /* (current_vertical_pos == current_horizontal_pos) */
	{
		/* NOTE: we invoke POSITION_TO_USEC twice below to keep the code paths
		 * the same length of time... otherwise the servos jitter when
		 * the vertical and horizontal degrees equal
		 */
		POSITION_TO_USEC(current_horizontal_pos, usecs);
		POSITION_TO_USEC(current_horizontal_pos, usecs);
		timer1_action = STOP_BOTH_PULSES;
	}

	/* the code below is not really needed if the vertical and horizontal
	 * degrees are the same... it is here to keep code paths the same,
	 * see NOTE above.
	 */
	timer1_usecs_remaining -= usecs;
	timer1_usecs_remaining = SERVO_INTERRUPT_TIME(timer1_usecs_remaining);
	th1_remaining = HI_BYTE(timer1_usecs_remaining);
	tl1_remaining = LOW_BYTE(timer1_usecs_remaining);

	/* timer1 time to stop shortest pulse */
	usecs = SERVO_INTERRUPT_TIME(usecs);
	TH1 = HI_BYTE(usecs);
	TL1 = LOW_BYTE(usecs);

	/* start pulse on both servo ports */
	HORIZONTAL_SERVO_ON();
	VERTICAL_SERVO_ON();

	/* start both timers */
	TCON |= 0x50;

#if HEART_BEAT_A7
	/* blink port A7, we should be running this ISR at 20msec
	 * intervals so below code should cause A7 to blink every second 
	 */
	{
#define ON_PERIOD 50    /* 50*20usec == 1sec */
		idata static int i = 0;
		if (i<ON_PERIOD) {
			if (i==0) {
				OUTA &= 0x7F; /* off */
			}
			i++;
		} else if (i<(2*ON_PERIOD)) {
			if (i==ON_PERIOD) {
				OUTA |= 0x80; /* on  */
			}
			i++;
			if (i==(2*ON_PERIOD)) {
				i=0;
			}
		} 
	}
#endif /* HEART_BEAT */

#if HEART_BEAT_C3
	/* blink port C3, we should be running this ISR at 20msec
	 * intervals so below code should cause C3 to blink every second 
	 */
	{
#define ON_PERIOD 50    /* 50*20usec == 1sec */
		idata static int i = 0;
		if (i<ON_PERIOD) {
			if (i==0) {
				OUTC |= 0x08; /* off (high) */
			}
			i++;
		} else if (i<(2*ON_PERIOD)) {
			if (i==ON_PERIOD) {
				OUTC &= 0xF7; /* on (low) */
			}
			i++;
			if (i==(2*ON_PERIOD)) {
				i=0;
			}
		} 
	}
#endif /* HEART_BEAT */

#if DATA_BEAT_A7
	if (OUTA & ~0x7F) /* A7 is on */ {
		idata static int i = 0;
		if (i++>=3) /* 60+ usec */ {
			OUTA &= 0x7F; /* turn port A7 off */
			i = 0;
		}
	}
#endif /* DATA_BEAT_A7 */

#if DATA_BEAT_C3
	if (!(OUTC & 0x08)) /* C3 is on (low) */ {
		idata static int i = 0;
		if (i++>=3) /* 60+ usec */ {
			OUTC |= 0x08; /* turn port C3 off (high) */
			i = 0;
		}
	}
#endif /* DATA_BEAT_A7 */

}

/****************************************************
 * horizontal servo timer
 ****************************************************/
static void 
timer1_isr(void) interrupt 3 using 2 critical
{
	/* stop timer1 */
	TCON &= ~0x40;

	/* NOTE: this is not efficient as it could be - the code is
	 * organized to keep the duration the same between actions.
	 * we still get a bit of jitter when the servo horizontal
	 * and vertical degrees cross...
	 */
	TH1 = th1_remaining;
	TL1 = tl1_remaining;

	if (timer1_action == HORIZONTAL_TIME_REMAINING) {
		TCON |= 0x40;
		VERTICAL_SERVO_OFF();
	} else if (timer1_action == VERTICAL_TIME_REMAINING) {
		TCON |= 0x40;
		HORIZONTAL_SERVO_OFF();
	} else /* STOP_BOTH_PULSES */ {
		HORIZONTAL_SERVO_OFF();
		VERTICAL_SERVO_OFF();
	}

	timer1_action = STOP_BOTH_PULSES;
}

/****************************************************
 * _sdcc_external_startup()
 ****************************************************/

byte 
_sdcc_external_startup()
{
	ISOCTL |= 0x01;  /* Disable ISO, see trm v1.9 page 3-3 (56)          */
						  /* this gives us the memory at 0x2000 to 0x27FF     */
	/*
	{_asm
		mov   sp,#0x2200;
	_endasm;
	}
	*/
	return (0);
}

/****************************************************
 * main ()
 * Basic initialization, re-enumeration and the reset
 ****************************************************/
int 
main(void)
{

	/* init variables */
	center_usecs    = SERVO_PULSE_TIME(CENTER_USECS);
	pulse_frequency = SERVO_PULSE_TIME(PULSE_FREQUENCY);
	current_horizontal_pos = 0;
	current_vertical_pos = 0;
	ep1in_pending = 0;
	pwm_message.new_command = 1;
	pwm_message.horizontal_pos = 0;
	pwm_message.vertical_pos = 0;
#if DELAY_PER_PERIOD
   servo_delay_count = 0;
#endif /* DELAY_PER_PERIOD */

	init_timers();
	reset_ezusb();

	/* blink_portA7(2, 1); */
	while (1) { };
}