autoroad.c

I/O board control program for U.C.W./Sirael Dana robot.

/*
 * I/O board control program for U.C.W./Sirael Dana robot.
 *
 * Version 1.0
 *
 * Copyright (c) 2002 Vojtech Pavlik
 * Copyright (c) 2001 Arnim Laeuger
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 2 of the License, or (at your option)
 * any later version. See also the file COPYING which came with this
 * application.
 */

#include <8051.h>
#include "ezusb_reg.h"
#include "dana-io.h"

static void init_variables()
{
	i2c_mode = 0;
	i2c_state = 0;
	i2c_index = 0;
	tx_head = 0;
	tx_tail = 0;
	tx_running = 0;
	in1_change = 0;
	beat = 0;
}

/*
 * get_descriptor()
 *
 * Sends the requested descriptor back to the host.
 * Currently supported descriptor types are:
 *   + device
 *   + configuration
 *   + string
 */

#pragma SAVE
#pragma NOOVERLAY
static void get_descriptor(void) using 1
{
   switch (sdat->wValueH) {
         /* type: device */
      case USB_DT_DEVICE:
         EP0CS   = 0x02;   /* clear HSNACK */
         SUDPTRH = (Byte)((unsigned int)dev_desc >> 8);
         SUDPTRL = (unsigned int)dev_desc & 0xff;
         break;

         /* type: configuration */
      case USB_DT_CONFIG:
         EP0CS   = 0x02;   /* clear HSNACK */
         SUDPTRH = (Byte)((unsigned int)conf_desc >> 8);
         SUDPTRL = (unsigned int)conf_desc & 0xff;
         break;

         /* type: string */
      case USB_DT_STRING:
         if (sdat->wValueL < NUM_STRING) {
            EP0CS     = 0x02;   /* clear HSNACK */
            SUDPTRH = (Byte)(string_index[sdat->wValueL] >> 8);
            SUDPTRL = string_index[sdat->wValueL] & 0xff;
         } else {
            EP0CS     = 0x01;   /* stall */
         }
         break;

      default:
         EP0CS     = 0x01;   /* stall */
         break;
   }
}
#pragma RESTORE

/*
 * usb_isr()
 *
 * USB interrupt service routine
 * Performs five major tasks:
 *   + parse the SETUP packets upon SUDAV interrupt and answer them
 *   + ack IN0 interrupts
 *   + ack IN1 interrupts
 *   + ack OUT0 interrupts
 *   + ack OUT1 interrupts
 */

static void usb_isr(void) interrupt 8 using 1 critical
{
   /* clear INT2 interrupt */
   EXIF &= 0xef;

   /* was this the SUDAV interrupt? */
   if (USBIRQ & 0x01) {

      /* clear SUDAV interrupt */
      USBIRQ = 0x01;

      switch (sdat->bRequest) {

            /* GetDescriptor */
         case 0x06:
            get_descriptor();
            break;

            /* GetConfiguration */
         case 0x08:
            if (sdat->bmRequestType == 0x80) {
               in0buf(0) = 0x01;
               IN0BC     = 0x01;
               EP0CS     = 0x02;   /* clear HSNACK */
            } else {
               EP0CS     = 0x01;   /* stall */
            }
            break;

            /* SetConfiguration */
         case 0x09:
            switch (sdat->bmRequestType) {
                  /* SetConfiguration */
               case 0x00:
                  EP0CS     = 0x02;   /* clear HSNACK */
                  break;

               default:
                  EP0CS     = 0x01;   /* stall */
                  break;
            }
            break;

         default:
            EP0CS     = 0x01;   /* stall */
            break;
      }

   }

   /* was this an IN0 interrupt? */
   if (IN07IRQ & 0x01) {
      /* clear IN0 interrupt */
      IN07IRQ = 0x01;
   }

   /* was this an IN1 interrupt? */
   if (IN07IRQ & 0x02) {
      /* clear IN1 interrupt */
      IN07IRQ = 0x02;
   }

   /* was this a OUT0 interrupt? */
   if (OUT07IRQ & 0x01) {
      /* clear OUT0 interrupt */
      OUT07IRQ = 0x01;
      /* arm endpoint again */
      OUT0BC = 0x00;
   }

   /* was this a OUT1 interrupt? */
   if (OUT07IRQ & 0x02) {

      /* clear OUT1 interrupt */
      OUT07IRQ = 0x02;

	if (out1buf(0) != 255) {

		/* Move servo to desired position */

		tx_buffer[tx_head] = 0xff;
		tx_head = (tx_head + 1) & (TX_BUFFER_SIZE - 1);
		tx_buffer[tx_head] = out1buf(0);
		tx_head = (tx_head + 1) & (TX_BUFFER_SIZE - 1);
		tx_buffer[tx_head] = out1buf(1);
		tx_head = (tx_head + 1) & (TX_BUFFER_SIZE - 1);

		if (tx_running == 0) {
			SBUF0 = tx_buffer[tx_tail];
	                tx_tail = (tx_tail + 1) & (TX_BUFFER_SIZE - 1);
			tx_running = 1;
		}
	} else {
		/* Switch power on/off */
		if (out1buf(1))
			OUTA |= 0x80;
		else
			OUTA &= 0x7f;
      }

      /* arm endpoint again */
      OUT1BC = 0;
   }

   /* was this the URES interrupt? */
   if (USBIRQ & 0x10) {
      /* clear request */
      USBIRQ = 0x10;
      /* initialize global variables */
      init_variables();
   }
}

static void serial_0_isr(void) interrupt 4 using 2 critical
{
	/* ack receiver interrupts (ignore) */
	RI_0 = 0;
	/* ack transmitter interrupts */
	TI_0 = 0;
	if (tx_head != tx_tail) {
		/* transmit queued data */
		SBUF0 = tx_buffer[tx_tail];
		tx_tail = (tx_tail + 1) & (TX_BUFFER_SIZE - 1);
		return;
	} 
	tx_running = 0;
}

/*
 * This routine is called on I2C transfer completion.
 */

static void i2c_isr() interrupt 9 using 3 critical
{
	/* Clear the i2c interrupt */

        I2CS;
	EXIF &= 0x10;

	switch (i2c_mode) {
	
		case I2C_PCF8591:

			switch (i2c_state++) {

				case 0:
					if ((I2CS ^ I2C_ACK) & (I2C_BERR | I2C_ACK)) goto fail;
					i2c_buffer[i2c_index + 0] = I2DAT;
					return;
				case 1:
					if (I2CS & I2C_BERR) goto fail;
					i2c_buffer[i2c_index + 0] = I2DAT;
					return;
				case 2:
					if (I2CS & I2C_BERR) goto fail;
					i2c_buffer[i2c_index + 0] = I2DAT;
					return;
				case 3:
					if (I2CS & I2C_BERR) goto fail;
					I2CS = I2C_LAST;
					i2c_buffer[i2c_index + 1] = I2DAT;
					return;
				case 4:
					if (I2CS & I2C_BERR) goto fail;
					I2CS = I2C_STOP;
					i2c_buffer[i2c_index + 2] = I2DAT;
					return;
			}

		case I2C_MAX127R:
		case I2C_MAX127W:
		case I2C_MAX1236:
			return;
	}
	
fail:
	I2CS = I2C_STOP;
}

#pragma SAVE
#pragma NOOVERLAY
static void i2c_start_read(Byte mode, Byte device) using 3
{
	i2c_mode = mode;
	i2c_state = 0;

	while (I2CS & 0x40);
	I2CS = I2C_START;

	switch (mode) {
		case I2C_PCF8591:
			i2c_index = device * 3;
			I2DAT = I2C_PCF8591_ADDR | (device << 1) | I2C_READ;	
			return;
		case I2C_MAX127R:
		case I2C_MAX127W:
		case I2C_MAX1236:
			return;
	}
}
#pragma RESTORE

/*
 * This routine is called every 4 ms.
 */

#define ABS(x)	((x)>0?(x):-(x))

static void timer2_isr() interrupt 5 using 3 critical
{
	Byte i;
	int x, y;

	TF2 = 0;

	beat++;

	if (!(beat & 0x3f))
		OUTC ^= 0x20;

	/* Read bumpers & switches */
	/* TODO: use interrupt */
	if (in1buf(0) != PINSB) {
		in1buf(0) = PINSB;
		in1_change = 1;
	}

	/* Read motor data */
	/* TODO: Process these, use interrupt! */
	if (in1buf(1) != PINSA) {
		in1buf(1) = PINSA;
		in1_change = 1;
	}

	for (i = 0; i < I2C_BUFFER_SIZE; i++) {
		x = i2c_buffer[i];
		y = in1buf(i + 8);
		if (x != y) {
			in1buf(i + 8) = x;
			in1_change = 1;
		}
	}

	if (in1_change & !(IN1CS & 0x02)) {
		IN1BC = IN1_BUFFER_SIZE; 
		in1_change = 0;
	}

	/* Start another I2C read cycle */
	/* TODO: read more than one sensor */
	i2c_start_read(I2C_PCF8591, 0);
}

static void setup_usb_int()
{
   /* disable global interrupt */
   EA       = 0;

   /* clear autovector enable */
   USBBAV   = 0x00;
#ifndef ROM_FIRMWARE
   /* clear SUDAV interrupt and all other interrupts */
   USBIRQ   = 0xff;
#endif
   /* enable SUDAV and URES interrupts */
   USBIEN   = 0x11;

#ifndef ROM_FIRMWARE
   /* clear IN0 and IN1 interrupts */
   IN07IRQ  = 0x03;
#endif
   /* enable IN0 interrupt only */
   IN07IEN  = 0x01;

#ifndef ROM_FIRMWARE
   /* clear OUT0 and OUT1 interrupt */
   OUT07IRQ = 0x03;
#endif
   /* enable OUT0 interrupt */
   OUT07IEN = 0x03;

#ifndef ROM_FIRMWARE
   /* unstall endpoints */
   IN1CS  = 0x00;
   OUT1CS = 0x00;
#endif

   /* validate endpoints */
   IN07VAL  = 3;
   OUT07VAL = 3;

   /* enable external 2 interrupt */
   EUSB = 1;

   /* enable global interrupt */
   EA   = 1;
}

static void setup_io()
{
	/* PA6, PA7 - out (motor, controllers) */
	/* all other pins on port A set to input */

	PORTACFG = 0x00;
	OUTA = 0x00;
	OEA = 0xC0;

	/* All pins on port B input (bumpers, switches) */

	PORTBCFG = 0x00;
	OUTB = 0x00;
	OEB = 0x00;

	/* RXD0,TXD0 enable, PC4,PC5 out (heartbeat) */
	/* all other pins on port C input */

	PORTCCFG = 0xcf;
	OUTC = 0x00;
	OEC = 0x30;
}

static void setup_i2c_int()
{
	/* clear INT3 interrupt */
	EXIF &= 0x10;
	/* clear I2C interrupt */
	I2CS;
	/* enable I2C interrupt */
	EIE |= 0x02;
}

static void setup_ser_int()
{

   /*
    * configure timer 1:
    * + mode 2
    * + clock divider 4
    * + SMOD 1
    * + reload value 0xd9 
    * + baud rate 9600 + 0.15%
    * + receiver disabled
    * + mode 1 (async 8N1)
    */
   TMOD     = 0x20;
   CKCON   |= 0x10;
   TH1      = 0xd9;