/* MAXMC11A echos command characters to terminal */
/* ROM version                 */
/* MAX MC11 TEST PROGRAM */
/* by AJ Neal */
/* includes sonar code */
/* includes encoder code */
/* Copyright 2000 - Zagros Robotics, All Rights Reserved */

/* set up for 32k eeprom */
#data 0x0000             /* Data segment is at $0000      */
#code 0x8000             /* Code segment is at $E000      */
#stack 0xFF              /* Base Address of Stack is $100 */

#include <init.k>
#include <68hc11.h>
#include <max.h>
#include <ser_in.c>
#include <ser_out.c>
#include <stdio.h>

    /* Link in the following libraries (nearly all 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>

/* main vars */        
char c;                   /* Declare char variable c       */
FILE stdout;

/* sonar vars */
unsigned int sonar_range;
unsigned int sonar_init;

/* advance , encoder vars */
int m1count;
int m1temp;
int m1old;



ping(unit)
int unit;
{       
        int sonar_temp;
        int sonar_old;
    
        pokeb(REG_BASE+PORTD,4); /* PING SONAR UNIT 1*/

        sonar_init=peek(REG_BASE+TCNT);
   
        sonar_range=0;
        do
        {

        sonar_range=peek(REG_BASE+TCNT);
        sonar_range=sonar_range-sonar_init;
        sonar_temp=peekb(REG_BASE+PORTA);
       
        }while(((sonar_temp&1)==0)||(sonar_range<6000));
        

        pokeb(REG_BASE+PORTD,0);


     
        return(sonar_range);
}/* end ping function */




advance(distance)
int distance;
 {
           m1count=0;
     

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

           }
        MOTOR('s'); /* stop motors */
          

        
      

} /* end advance */

help()
{
                 fprintf(stdout,"Zagros Robotics version 5/07/2000\n");
                 fprintf(stdout,"MAXMC11A Commands\n");
                 fprintf(stdout,"(f) - Forward\n");
                 fprintf(stdout,"(b) - Backward\n");
                 fprintf(stdout,"(l) - Left Rotate\n");
                 fprintf(stdout,"(r) - Right Rotate\n");
                 fprintf(stdout,"(s) - Stop\n");
                 fprintf(stdout,"(a) - Advance 10 encoder counts\n");
                 fprintf(stdout,"(p) - Ping Sonar Unit\n");
                 fprintf(stdout,"(t) - Toggle PD2\n");
                 fprintf(stdout,"(h) - Help Menu\n");
} /* end help */


          main()                    /* Begin of main routine         */
          {
             int dist;
             stdout=fopen(SER_out);
             pokeb(REG_BASE+BAUD,0x30); /* Set up the serial channel     */
             pokeb(REG_BASE+SCCR2,0x0C);
             pokeb(REG_BASE+DDRD,12); /* setup 2,3 bits as outputs */
             pokeb(REG_BASE+SPCR,4); /* make sure bit DWOM is 0 for CMOS output */

             fprintf(stdout,"Zagros Robotics MaxMc11 version 5/07/2000\n");
             fprintf(stdout,"Copyright 2000 - Zagros Robotics\n");
             fprintf(stdout,"All Rights Reserved\n");
             help();

             /* set porta for motor control bit 3 as output */
             pokeb(0x1026,0x88);/* setup bit 3 as output */
             /* set clock to slowest speed */
             /* 524.3ms / 64k counts */  
             #asm
                LDX #$1024
                BSET 0,X $03
             #endasm

             /*pokeb(0x1000,BACK);*/
             MOTOR('s');
             while(c!='q')                /* Create an infinite loop       */
             {
                c=SER_in();          /* Read a char from serial       */
                SER_out(c);          /* Write a char to serial        */
                MOTOR(c);
                if (c=='a')
                {
                MOTOR('f');
                advance(20);   /*if motors are moving forward...go for 20 counts */
                }

                if (c=='p')
                {
                       fprintf(stdout,"%u\n",ping(1));
                }
                /* toggle bit PD@ */
                if (c=='t')
                {
                dist=1-dist;
                if (dist==1)
                {
                pokeb(REG_BASE+PORTD,4); /* PING SONAR UNIT 1*/
                fprintf(stdout,"PD2 on\n");
                }
                if (dist==0)
                {
                pokeb(REG_BASE+PORTD,0); /* RESET SONAR UNIT 1*/
                 fprintf(stdout,"PD2 off\n");
                }
                } /* END TOGGLE */
                      /* help menu */
                if (c=='h')
                {
                 help();
                 } /* end help menu if*/

             } /* end while */
          MOTOR('s');
          }                          /* End of main routine           */