navigation.c

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

//  Navigation     Navigation.c					// Rev 9/1/05

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

/*
   This module performs low level navigation functions.
	 It calculates current speed (mm/sec) and distance traveled	(mm).
	 Also calculates coordinates on an X/Y grid in mm.
*/
	 										
#include "Microcontroller.h"	  //for registers, config data & stdio
#include <math.h>				  //for trig functions (float)

  extern long enc0;   		  	  //drive wheel encoder, normally left wheel
  extern long enc1;   		  	  //drive wheel encoder, normally right wheel 
  
  extern int hdgdeg;			  //compass heading from DG.c
  extern int time;
  
  long odomL,odomR;		  		  //L&R odometer values (encoder clicks)
  long odomLlast,odomRlast;  	  //temporary values of previous odom data
  int  SpeedL,SpeedR;         	  //Left/Right wheel speed mm/sec.
  int  Speed;        			  //Average wheel speed mm/sec.
  long  DistActL,DistActR;		  //Left/Right distance traveled mm.
  long  DistAct;				  //average distance wheels traveled mm.
  
  long odomLnum   = 10000;	  //default wheel calibration values
  long odomLdenom = 10000;    // normally laptop will download values
  long odomRnum   = 10000;	  // to replace these.
  long odomRdenom = 10000;  
	
long Xcoord,Ycoord;			  //X/Y coordinates
  int  headingDR;				  //encoder based heading (not impl yet)
  float hdgf; 	  				  //heading in radians
  float xf,yf;
  float delf;
  float sinf,cosf;
  
void odometry(void)
  {  
	long temp1,temp2;	   	 		  //temporaries used in DistAct calculations
	//printf("odo %ld  %ld  %ld  %ld  ",odomLnum,odomLdenom,odomRnum,odomRdenom);
	//assigns odometry to proper encoder input.
   	 odomL = -enc0;  	   //just matches up odometry to the correct encoders
	odomR =  enc1;		   //and adjusts sign if necessary
	
  	//calculate individual wheel speeds and average
    //calibration constant = 61.035 * DistAct  factors below for each wheel
	// calc is *2 so can round up or down based on lsb.
	SpeedL = (odomL - odomLlast) * 122*odomLnum/odomLdenom; //speed in mm/sec
	if(SpeedL & 0x0001) SpeedL =SpeedL/2+1; else SpeedL = SpeedL/2;
	odomLlast = odomL;
				   						   
	SpeedR = (odomR - odomRlast) * 122*odomRnum/odomRdenom; 
	if(SpeedR & 0x0001) SpeedR =SpeedR/2+1; else SpeedR = SpeedR/2;
	odomRlast = odomR;
	
	Speed  = (SpeedL + SpeedR) / 2;						   //average speed				

  	//calculate actual distances 
    	//The fudge factors below result from calibration tests to make the 
	//robot go straight and the correct distance. 
	//DistAct = odom * odomxnum/odomxdenom.  The somewhat complex
	//calculation provides that full range of distance is accounted for.
	
	temp1 = odomL/odomLdenom;
	temp2 = odomL - temp1 * odomLdenom;
	DistActL = temp1 * odomLnum + temp2 * odomLnum/odomLdenom; //distance in mm.
			   		 							  			   //range +/- 200 km.
    	temp1 = odomR/odomRdenom;
	temp2 = odomR - temp1 * odomRdenom;
	DistActR = temp1 * odomRnum + temp2 * odomRnum/odomRdenom;

	DistAct = (DistActL+DistActR)/2;						   //average distance

	//calculate X & Y coordinates based on speed and compass heading.
	//  done in floating point for trig functions.
	hdgf = hdgdeg/572.9578;   					//Compass/10/57.3 = radians
	xf += (Speed * cos(hdgf))/61;				//integrate on speed and direction
	yf += (Speed * sin(hdgf))/61;
	Xcoord = xf;   				 				//convert back to integer
	Ycoord = yf;
  }

  void SetX(long x)		   //laptop can correct X and Y calculations
  {
    Xcoord = x;
	xf = Xcoord;
  }
  
  void SetY(long y)
  {
	Ycoord = y;
	yf = Ycoord;
  }	
  
//  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.
*/