bite.c

老外开发的机器人的底层单片机代码。比较有参考价值哦！

//  Built in test   BITE.c					// Rev 12/31/05

//Copyright (C) 2005 Alex Brown	[e-mail]
//This program is free software; See license at the end of this file for details.


#include <hcs12dp256.h>
#include <stdio.h>   //for printf

//Declarations
void IRsensors(void);
void MotorFwd(int Acc, int Vmx, long int DistTarg, int ResetOdom); 
void MotorStop(int Acc); 
void RCservoScan(int,int,int,int);
void RCservoGoTo(int,int,int);

char* gets(char*);			  //get string function in this module

//Miscellaneous
//   extern int i,j,k;   //misc temp variables
   extern int time;
   extern int BiteMode;

//Compass and Directional Gyro
   extern int  Compass;	  	  //compass angle 0 to 3599 (deg*10)
   extern int  NullAvg;		  //rate gyro null
   extern int  YRatedps;	  //yaw rate in deg/sec *10
   extern int  hdgdeg;		  //directional gyro heading in deg*10		  
//sonar sensors
   extern int RC_Sonar[7];   
//accelerometer/tilt sensors
   extern int PitchDeg,RollDeg; 
   extern int AccelX,AccelY;  
//motor
   extern int FwdModeDone;	  // = 1 when a forward command has completed
   extern long enc0,enc1;	  // values of wheel encoders
   extern long odomL, odomR;
   extern int PWMtestOn;	  // = 1 when test overrides normal pwm command
   extern int testvalueL;	  // test pwm values for Left and right motors 
   extern int testvalueR;	  //     0 to 100 (forward) or -100 (reverse)
   extern int PWMmax;
   
void Bite(void)
 {
  int BiteCmd;
  int i,j;	   		   //temporary counters
  char ch;			   //char used in reading terminal input
  char instr[20];	   //string to hold terminal input
  
  for (i = 0; i<20;i++) printf("\n");
  printf(" Robot Controller test program\n");
  printf("\n");
  printf(" 1:   Print IR sensor values\n");
  printf(" 2:   Compass and Directional Gyro\n");
  printf(" 3:   Check motor drive and encoder\n");
  printf(" 4:   Accelerometer/Tilt sensor\n");
  printf(" 5:   Check SRF04 sonar\n");
  printf(" 6:   RC servo scan\n");
  printf(" 7:   RC servo goto\n");
  printf(" 8:   Motor PWM test\n");
  printf(" 9:	exit BITE mode\n");
  printf("\n");
  printf("Input a single number to select:\n");
  printf(" \nHit Esc to exit any mode \n");

  BiteCmd = getchar(); 	 //get input command
  BiteCmd -= 48;         //convert from ascii to integer
  
   switch (BiteCmd)
    
   {case 1:	 	  	  	 // IR sensors
     i = 0;
	 j = 10;
     while (i !=27) 		  	  //while not escape key
      { if (j==10) 
	     {printf(" IRrange0 IRrange1 IRrange2 IRrange3 IRrange4 IRrange5 IRrange6 IRrange7\n");
		  j = 0;
		 }
		else j = j+1;
//		printf(" %d      %d       %d      %d      %d      %d      %d      %d\n",
//		IRrange0,IRrange1,IRrange2,IRrange3,
//		IRrange4,IRrange5,IRrange6,IRrange7);
		time = 0;  
		while (time < 61) ;	  	  	  	   //pause 1 second
	    if (SC0SR1 & 0x20) i = getchar();  //check for keyboard command	  
       };
	  break;  	

    case 2:	 	  	  	 // Compass and Directional Gyro
     i = 0;
	 j = 10;
     while (i !=27) 		  	  //while not escape key
      { if (j==10) 
	     {printf(" Compass   RG null     Yaw Rate      heading\n");
		  j = 0;
		 }
		else j = j+1;
		printf(" %d        %d         %d            %d \n",
		Compass,NullAvg,YRatedps,hdgdeg);
		time = 0;  
		while (time < 61) ;	  	  	  	   //pause 1 second
	    if (SC0SR1 & 0x20) i = getchar();  //check for keyboard command	  
       };
	  break;  	

	case 3:
	
	  time = 0;
	  printf(" \n robot will start in 10 seconds");
	  while (time < 610) ;  //wait 10 seconds
	  MotorFwd(300,200,1000,1);  //go fwd at 200 mm/sec for 1000 mm.
	  printf(" Going Forward\n");
	  while(FwdModeDone == 0);  //wait til done
	  MotorStop(300);
	  MotorFwd(-300,-200,-1000,1);  //go back at 200 mm/sec for 1000 mm.
	  printf(" Going in Reverse\n");
	  while(FwdModeDone == 0);  //wait til done
	  MotorStop(300);
	  break; 

    case 4:	 	  	  	 // Accelerometer / Tilt sensor
     i = 0;
	 j = 10;
	 
	//init timer in case not init by laptop
	TCTL1 = 0x80;
	TIOS  = 0x80;
	TSCR2 = 0x0C;        //overflow intr inhibited, timer reset on OC7,
		  				//   prescale = 16
	TC7   = 30000;		//20 ms period
	TSCR1 = 0xc0; 		//turn timer on

     while (i !=27) 		  	  //while not escape key
      { if (j==10) 
	     {printf(" Lateral accel      Longitudinal accel\n");
		  j = 0;
		 }
		else j = j+1;
		printf("      %6d                 %6d \n", AccelX,AccelY);
		time = 0;  
		while (time < 61) ;	  	  	  	   //pause 1 second
	    if (SC0SR1 & 0x20) i = getchar();  //check for keyboard command	  
       };
	  break;  	

	case 5:			  		//check SRF04 sonars
	  i = 0;
	  time = 0;
	  while (i != 27)
	   {while  (time < 61);  //delay one second
		printf("Sonars 0 to 6:   %6d   %6d   %6d   %6d   %6d   %6d   %6d\n",
						  RC_Sonar[0],RC_Sonar[1],RC_Sonar[2],RC_Sonar[3],
						  RC_Sonar[4],RC_Sonar[5],RC_Sonar[6]);
		time = 0;
         if (SC0SR1 & 0x20) i = getchar();
	   };
	  break; 
	  
    case 6:	 	  	  	 // RC servo test.  Scans back and forth
	 printf("RC servo scan test in progress\n");
	 RCservoScan(4,900,-900,1800); //scan between +/- 90 degrees at 30 dps.
	 RCservoScan(5,900,-900,600);
	 RCservoScan(6,200,-200,100);
	 
     while (i !=27) 		  	  //while not escape key
      { 
	   if (SC0SR1 & 0x20) i = getchar();  //check for keyboard command	  
      };
	 RCservoGoTo(4,0,300); 	//goto zero degrees when done
	 RCservoGoTo(5,0,300); 
	 RCservoGoTo(6,0,300); 
	  break;  	
	   
		

    case 7:	 	  	  	 // RC servo test.  alternates between 2 positions
	 printf("RC servo goto test in progress\n");
	 printf("  hit space bar to toggle position, hit ESC to exit");
	 RCservoGoTo(4,900,1800); // go to 60 degrees at 30 dps.
	 RCservoGoTo(5,-600,300);
	 RCservoGoTo(6,-450,100);
	 j=1;
	 while(1)
	 {
	  	while(!SC0SR1 & 0x20) ;   // wait for a keypress
		i = getchar();
		if(i == 27) break;	  	  // exit loop if ESC key
		if(j == 0)
		{
		  RCservoGoTo(4,900,1800); // go to 60 degrees at 30 dps.
	 	  RCservoGoTo(5,-600,300);
	 	  RCservoGoTo(6,-450,100);
  		  j = 1;
		}
		else  
		{
		  RCservoGoTo(4,-900,1800); // go to 0 degrees at 30 dps.
	 	  RCservoGoTo(5,0,300);
	 	  RCservoGoTo(6,450,100);
  		  j = 0;
		}
	 }
	 break;	   	   				  // exit case
	   
	case 8:				 // Motor PWM test (contributed by John Edwards)
	
	  PWMtestOn = 1;	  		//Set test flag to on
	  time = 0;
	  if(PWMmax == 0) PWMmax = 100;
	  testvalueL = 0;			//initiate motors off
	  testvalueR = 0;			//initiate motors off
	  printf("\n \n \n \n \n \n \n \n \n \n \n \n \n \n");
	  printf("Block wheels up off ground before test\n");
	  printf("to prevent robot from running off!\n\n");
	  printf("Input two test PWMs for left & right motors\n");
	  printf("Entries are limited to +/-100 percent\n");
	  printf("Negative numbers run motors in reverse\n\n");
	  printf("Prints encoder values during test\n");
	  printf("Hit Esc to end test\n\n");
	  printf("Enter Left & Right PWM values:(e.g. ""50 60""): ");
	  
  	  gets(instr);	   			  	 			  	   // read command string
	  sscanf(instr,"%d %d",&testvalueL,&testvalueR);   // extract values
	  if(testvalueL >  100)testvalueL =  100;		   // limit to +/- 100
	  if(testvalueL < -100)testvalueL = -100;
	  if(testvalueR >  100)testvalueR =  100;
	  if(testvalueR < -100)testvalueR = -100;

	  while (i !=27) 		  	  //while not escape key
      { 
	   printf("\n left encoder %ld right encoder %ld", odomL,odomR);
	   if (SC0SR1 & 0x20) i = getchar();  //check for keyboard command	  
      };
	  testvalueL = 0;
	  testvalueR = 0;
	  printf("\n\nBoth wheels stopped\n");
	  time = 0;
	  printf("\nHit any key to return to main menu\n");
	  getchar();
	  enc0 = 0; enc1 = 0;
	  MotorStop(300);	  
	  while (time < 61);  
	  PWMtestOn = 0;
	  break; 
	   	

	case 9:	 	  		   	   //exit BITE
	  time = 0;
	  BiteMode = 0;
	  printf(" Returned to normal run mode\n");
	  printf(" Hit Esc to enter Bite mode\n ");
	  break;
	    
	} 		  	   			  	   //close switch 	  
 }  

// gets()   similar to ANSI standard get string function
/*   reads characters from serial port and places them in the string pointed
     at by the parameter.  Ends when Enter is pressed and terminates the string
     with a '0'.  
     It echos the characters to the terminal and handles the backspace key. 
*/
char* gets(char* str)
{
  char ch;		 	 		 // character read from serial port
  char *ptr;				 // pointer to string
  
  ptr = str;				 // initialize pointer to start of string
  
  for ( ; ; )
  {
    ch = getchar();	 		 // get input character
	
	if(ch == '\b') 	 		 // if backspace key
	  {if(ptr > str)		 	 // ignore if points to first char
	    { ptr--; 			 	 // back up string pointer
		  putchar(ch);			 // echo character to terminal
		}
	  }	
	else if (ch == '\r')     // if Enter key
	  { putchar(ch);			 // echo character to terminal
	    *ptr = '\0';	 		 // terminate string 
	    break; 					 // and exit loop
	  }	
	else 
	  { putchar(ch);			 // echo character to terminal
	    *ptr++ = ch;			 // else put character in string & incr pointer
	  }	
  }	
  return str;
}    

//  OPEN SOURCE SOFTWARE LICENSE
/* Permission is hereby granted, free of charge, to any person obtaining a copy 
of this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to use, 
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
Software, and to permit persons to whom the Software is furnished to do so, 
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/