/*   ICCDEMO.C                  */
/*   Copyright 2001 - Zagros Robotics                */
/*   version 11/10/2001 */
/* HC11 MC  */
/* PA0 sonar echo return */
/* PA1 encoder input (main) */ 
/* PA2 encoder input */
/* PA3 enable A */
/* PA4 enable B */
/* PA5 dir A    */
/* PA6 dir B    */
/* PD2 - sonar ping 1 */
/* PD3 - sonar ping 2 */
/* PD4 - sonar ping 3 */
/* PE0 - compass */
/* PE1 - compass */
/* PE2 - compass */
/* PE3 - compass */

#include <hc11.h>
#include <stdio.h>
#include "vectors.c"


/* motor control commands */
#define FORWARD 24
#define BACK 120
#define LEFT 56
#define RIGHT 88
#define STOP 0

void main(void)
	{
	int command;
	unsigned int temp;
	int dummy;
	setbaud(BAUD9600);
	/* set up PORTA bit 3 for output */
	PACTL ^=0x88;
	help();
	/* test output to port a*/
	PORTA ^=FORWARD;
	DDRD |=4; /* set bit 4 as output for sonar unit */
	PORTA = STOP;
	setbaud(BAUD9600);
	while(command !=42)
	{
	
    command=getchar();
	
	switch(command)
     {
        case 'f':
		   
           PORTA = FORWARD;
		   printf("...forward \n");
        break;
        case 'b':
           PORTA = BACK; 
		    printf("...backward \n");
        break;
        case 'l':
           PORTA = LEFT;
		    printf("...left \n");
        break;
        case 'r':
           PORTA = RIGHT;
		    printf("...right \n");
        break;
        case 's':
           PORTA = STOP; /* turn all bits to 0 */
		    printf("...stop \n");
        break;
		case 'p':
           temp=ping();
		   printf("RANGE :%d\n",temp);	 	   
        break;
		case 'a':
		   PORTA =FORWARD;
		   printf("(f)orward \n");
		   distance(36);	   
        break;
		case 'd':
			 printf("%u\n",PORTE);
	    break;
		case 'w':
		   do
		   {
		   temp=ping();
		   printf("RANGE :%d\n",temp);	
		   if (temp<6000)
		   PORTA=BACK;
		  else
		   if (temp<8000)
		   PORTA=RIGHT;
		   else
		   {
		   
		   PORTA=FORWARD;
		   }
		   /* a short delay */
		   dummy=0;
		   
		   while(dummy<32000)
		   { 
		   dummy++;
		  
		   } /* outer delay loop */
		   
		   }while(command=='w');	 
		
		break;	 
		case 'h':
        help();
        break;
       } /* end switch */
	 
	} /* end quit while */
	  printf("program ended...\n");
	exit (1);
	
    }

unsigned int ping()
{       
        unsigned int sonar_temp;
        unsigned int sonar_old;
        unsigned int sonar_init;
		unsigned int sonar_range;
		PORTD |=4; /* PING SONAR UNIT */
	
	    sonar_init=TCNT;
   
        sonar_range=0;
        do
        {

        sonar_range=TCNT;
        sonar_range=sonar_range-sonar_init;
        sonar_temp=PORTA;
       
        }while(((sonar_temp&1)==0)||(sonar_range<3800));
        
         PORTD &= ~0X04;
             
        return(sonar_range);
}/* end ping function */

int help()
{
 	printf("Zagros Robotics ICCDEMO version 9/2/2001\n");
	printf("Copyright 2001 - Zagros Robotics\n");
	printf("All Rights Reserved\n");
	printf("MENU\n");
	printf("(f)orward \n");
	printf("(b)ackward \n");
	printf("(l)eft \n");
	printf("(r)ight \n");
	printf("(s)top \n");
	printf("(p)ing \n");
	printf("(a)dvance \n");
	printf("(w)ander \n");
    printf("(h)elp \n");
	return(0);
}

void motor_speed()
{
unsigned int motor1_speed=25000;
unsigned int motor2_speed=25000;
unsigned int a=0;
int b=1;
unsigned int range;
int temp_a;
int temp_b;

/* get the speed */
while(b==1)
{
a=0;

while(a<10000)
{
   	 if (TCNT<motor1_speed)
	 { 
   	   temp_a=16;	
	 }
	 else
	 {
	 temp_a=0;
	 }
	 if (TCNT<motor2_speed)
	 { 
   	  temp_b=8;	
	 }
	  else
	  {
	  temp_b=0;
	  }
	PORTA=temp_a+temp_b;
	
	
a++;
}
range=ping();
printf("%d %d\n",motor1_speed,range);

if ((range>6000)&(motor1_speed>20000))
motor1_speed=motor1_speed-1000;
if ((range<7000)&(motor1_speed<30000))
motor1_speed=motor1_speed+1000;
}


} /* end function */

int distance(int distance)
 {
 
 /* advance , encoder vars */
int m1count;
int m1temp;
int m1old;
          
		  
		   m1count=0;
     

           while(m1count<distance)
           {
                m1temp=PORTA;
                printf("%d\n",m1temp);
                m1temp=m1temp&2;
                if(m1temp!=m1old)
                {
                m1old=m1temp;
                if(m1temp!=0)
                m1count++;
                printf("DIST:%d\n",m1count);
                }

           }
        PORTA =STOP; /* stop motors */
          

        
   return(0);   

} /* end advance */