/* MAX01 - USED TO TEST PORTS AND MOTOR CONTROL */
/* FORTH version */
/* copyright 1999 - Zagros Software */
/* define where the program should reside */
#code 0x0800   /* make sure RAM is installed on U2 socket */
#data 0x0700

/*
PORTA ASSIGNMENTS
PA0 - SONAR ECHO INPUT
PA1 - ENCODER INPUT 1
PA2 - ENCODER INPUT 2
PA3 - MOTOR 1 ON
PA4 - MOTOR 2 ON
PA5 - MOTOR 1 DIRECTION
PA6 - MOTOR 2 DIRECTION
PA7 - SONAR UNIT 5 INIT
PORTD ASSIGNMENTS
PD0 - RS232
PD1 - RS232
PD2 - SONAR UNIT 1 INIT
PD3 - SONAR UNIT 2 INIT
PD4 - SONAR UNIT 3 INIT
PD5 - SONAR UNIT 4 INIT
PORTE ASSIGNMENTS
PE0 - COMPASS INPUT 1
PE1 - COMPASS INPUT 2
PE2 - COMPASS INPUT 3
PE3 - COMPASS INPUT 4
PE4 - BUMP SWITCH INPUT 1
PE5 - BUMP SWITCH INPUT 2
PE6 - LIGHT SENSOR INPUT
PE7 - OPEN
*/

#include <forth.k>
#include <68hc11.h>
#include <stdio.h>

/* libraries needed by fprintf() */
#include <fopen.c>
#include <fputs.c> 
#include <fputc.c> 
#include <itoa.c>
#include <atoi.c>
#include <is.c>
#include <fprintf.c>
#include <itoab.c>
#include <reverse.c>
#include <strlen.c>
#include <delay.c> 

/* Link in the device driver for the 68hc11 */
#include <FOR_in.c>
#include <FOR_out.c>
char a;
int temp;
int count=0;
int old=0;
int c;
int point=0; /* compass var */
int m1count=0;
int m2count=0;
int m1old=0;
int m2old=0;
int m1temp=0;
int m2temp=0;
int sonar_range=0;
int sonar_init=0;
int dist=0;
int fire=0;
FILE stdout;

ping(unit)
char unit;
{
        pokeb(REG_BASE+DDRD,60); /* setup last for bits as outputs */

        switch(unit)
        {
        case '1':
        pokeb(REG_BASE+PORTD,4); /* PING SONAR UNIT 1*/
        break;
        case '2':
        pokeb(REG_BASE+PORTD,8); /* PING SONAR UNIT 2*/
        break;
        case '3':
        pokeb(REG_BASE+PORTD,16); /* PING SONAR UNIT 3*/
        break;
        case '4':
        pokeb(REG_BASE+PORTD,32); /* PING SONAR UNIT 4*/
        break;
        case '5':
        pokeb(REG_BASE+PORTA,128); /* PING SONAR UNIT 5 PA7*/
        break;

        } /* end case */
/*        while(peek(REG_BASE+TCNT)!=10); /* WAIT FOR COUNTER TO RESET */
        sonar_init=peek(REG_BASE+TCNT);
        sonar_range=0;

        while( ( (peekb(REG_BASE+PORTA)&1) !=1)&&(sonar_range<30000))
        {
        sonar_range=peek(REG_BASE+TCNT)-sonar_init;         
        }
        fprintf(stdout,"RANGE:%d\n",sonar_range); 
        pokeb(REG_BASE+PORTD,0);
        pokeb(REG_BASE+PORTA,0);
        return(sonar_range);

}/* end ping function */


main()
{
 stdout=fopen(FOR_out);
 pokeb(REG_BASE+DDRD,60); /* setup last for bits as outputs */
 pokeb(REG_BASE+PACTL,136); /* SET PORTA BIT 3 AS OUTPUT */
 pokeb(REG_BASE+PORTA,0);



fprintf(stdout,"ENTER COMMAND\n");

while(a!='Q')
{
        point=peekb(REG_BASE+PORTE);
        fprintf(stdout,"\n[%d] ENTER COMMAND :",point);
        a=FOR_in();
        if (a=='F')
        pokeb(REG_BASE+PORTA,0x60);
        if (a=='B')
        pokeb(REG_BASE+PORTA,0x78);
        if (a=='L')
        pokeb(REG_BASE+PORTA,0x68);
        if (a=='R')
        pokeb(REG_BASE+PORTA,0x70);
        if (a=='S')
        pokeb(REG_BASE+PORTA,0);
        if(a=='A') /* advance forward 10 encoder counts */
        {
           m1count=0;
        pokeb(REG_BASE+PORTA,0x60);

           while(m1count<10)
           {
                m1temp=peekb(REG_BASE+PORTA);
                m1temp=m1temp&2;
                if(m1temp!=m1old)
                {
                m1old=m1temp;
                if(m1temp!=0)
                m1count++;
                fprintf(stdout,"DIST:%d\n",m1count);
                }

           }
        pokeb(REG_BASE+PORTA,0x00);
          

        
        } /* end advance */


        if (a=='P')
        {

        fprintf(stdout,"\n ENTER UNIT :");
        a=FOR_in();
        fprintf(stdout,"DIST:%d\n",ping(a));


        
        } /* END P IF */


        /* explore a little bit */
        if (a=='X')
        {
                while(a=='X') /* forever while */
                {
                        

                        dist=ping('1');

                                if(dist<5100)
                                {
                                /* go backwards */
                                pokeb(REG_BASE+PORTA,0x78);
                                }
                                else
                                {
                                        if(dist<6000)
                                        {
                                        /* turn left */
                                        pokeb(REG_BASE+PORTA,0x68);

                                        }
                                        else
                                        {
                                        /* go forward */
                                        pokeb(REG_BASE+PORTA,0x60);
                                        }
                                }
                 delay(250);
                 } /* end explore while */

         }  /* end explore if */

/* COMPASS EXAMPLES */

        if (a=='N')
           {

                /* rotate left */
                pokeb(REG_BASE+PORTA,0x68);
                /* filter out compass bits */
                while((peekb(REG_BASE+PORTE)&15)!=14);
                /* stop in this position */
                pokeb(REG_BASE+PORTA,0x00);
               
           }







} /* end forever while */

/* STOP ALL MOTORS! */

pokeb(REG_BASE+PORTA,0);
delay(200);
fprintf(stdout,"SHUT DOWN COMPLETE\n");
}/* end main */