accel.c

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

//  Accelerometers   Accel.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.

/*					 			 			
      Takes data from dual axis accelerometer and filters and compensates it 
	to generate pitch and roll tilt angles.
	  Pitch is X axis acceleration filtered to reduce noise and converted to
	angle using small angle approximations.  Pitch is compensated to remove 
	effects of acceleration due to drive motors by subtracting out the 
	commanded accel or decel.
	  Roll is Y axis acceleration filtered and converted the same as Pitch.
	Roll is compensated by calculating and subtracting the lateral acceleration
	due to the robot pivoting or turning.
	
	Note: the two compensations noted above are to be in a future revision.
		  Right now, it just supplies plain filtered accel signals.
*/

#include <stdio.h>				//for i/o prototypes

int PitchDeg;		//pitch attitude angle in deg * 10 (e.g. 250 is 25.0 degrees)
int RollDeg;		//roll attitude angle in deg * 10
int AccelX;			//accel along robot longitudinal axis (not accel chip axis)
int AccelY;			//accel along robot lateral axis (not accel chip axis)

extern int AccIntX,AccIntY;  //dual axis accel raw data from PORTJ_handler()

void Tilt(void)
  {
    static long AccXfilt,AccYfilt;	 //acceleration lag filter outputs
	int AccIntX2, AccIntY2;		 //copies to avoid being changed by interrupt
	
	AccIntX2 = AccIntX;
	AccIntY2 = AccIntY;
	if(AccIntX2 < 0) AccIntX2 += 30000;	   //correct timer rollover
	if(AccIntY2 < 0) AccIntY2 += 30000;
	
	AccXfilt = (AccXfilt * 29)/30 + AccIntX2;   //lag filter 0.5 sec time const.
	  //add term above to subtract out commanded longitudinal accel
	AccYfilt = (AccYfilt * 29)/30 + AccIntY2;   //lag filter 0.5 sec time const.
	  //add term above to subtract out lateral accel due to motion
	AccelX = AccXfilt/30;  //add conversions to mm/sec^2???
	AccelY = AccYfilt/30;  
	PitchDeg = 0;//AccXfilt/30; //add conversions to degrees. 
	RollDeg  = 0;//AccYfilt/30; //   Do this in laptop???
  }

//  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.
*/