223 lines
9.3 KiB
C++
223 lines
9.3 KiB
C++
/*
|
|
Adept MobileRobots Robotics Interface for Applications (ARIA)
|
|
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
|
|
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
|
|
Copyright (C) 2011, 2012, 2013 Adept Technology
|
|
|
|
This program is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
|
If you wish to redistribute ARIA under different terms, contact
|
|
Adept MobileRobots for information about a commercial version of ARIA at
|
|
robots@mobilerobots.com or
|
|
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
|
|
*/
|
|
#ifndef ARANALOGGYRO_H
|
|
#define ARANALOGGYRO_H
|
|
|
|
#include "ariaTypedefs.h"
|
|
#include "ariaUtil.h"
|
|
#include "ArFunctor.h"
|
|
|
|
class ArRobot;
|
|
class ArRobotPacket;
|
|
|
|
/// Use onboard gyro to improve the heading in an ArRobot object's pose value
|
|
/**
|
|
The gyro is an accessory connected to the robot's microcontroller.
|
|
The gyro's purpose is to improve large errors due to wheel
|
|
slippage, wheels coming off the ground, and other factors like this
|
|
(or for the skid steer of an AT). The gyro also happens to provide
|
|
temperature readings as well.
|
|
|
|
When an ArAnalogGyro object is created, it registers callbacks with the
|
|
robot, and automatic heading correction will begin when new data arrives
|
|
from the robot.
|
|
If you delete the object, it will remove itself from the robot.
|
|
|
|
The readings come back from the gyro over the robot microcontroller's analog ports.
|
|
The microcontroller then sends the readings to us just before
|
|
sending us the standard packet. ArAnalogGyro uses these readings to
|
|
calculate a better heading of the robot (integrating the velocities to
|
|
give position). Then when the standard packet comes in, ArRobot calls
|
|
an encoder correction callback in ArAnalogGyro which does a simple
|
|
Kalman filter and fuses the encoder information's heading with the gyro's heading
|
|
to compute the most probable heading which it then returns to the
|
|
ArRobot object to use as its heading.
|
|
|
|
The robot's normal dead reconing angle is fairly good if you have properly
|
|
inflated tires (if you have pneumatic tires) and if you have
|
|
the revcount parameter sent correctly. See the robot operation manual for
|
|
how to change the revcount parameter.
|
|
|
|
Gyro readings are affected by temperature (a temperature value
|
|
is reported along with the gyro data in the packets from the
|
|
microcontroller). The gyro will auto calibrate itself to the
|
|
center of the range (which may have drifted due to temperature)
|
|
whenever the robot is stationary for more than one second (that is, if its
|
|
translational and rotational velocities are less than 1
|
|
and if the gyro readings are also within .5% of the average). The
|
|
scaling factor (between change of voltage and amount of turn)
|
|
doesn't seem to change due to temperature, though it can vary among
|
|
different gyro devices. (This scaling factor can be accessed with
|
|
getScalingFactor() and setScalingFactor().)
|
|
The default value used here was within 3% of correct most of the time.
|
|
The default resides in the parameter file for the robot that is loaded or used when a
|
|
connection is made. If you want to tune this more you can, by
|
|
finding a different scale factor. (There is just one so there
|
|
doesn't have to be alot of calibration in flash that is lost if
|
|
parameters are loaded from a file.)
|
|
|
|
General notes on gyros and inertial correction: The purpose of the gyro is to
|
|
correct for wheel slippage and large errors. It is not to totally correct
|
|
for all errors, but to bring the errors into smaller zones that
|
|
software can correct for more easily (such as by localization).
|
|
We have found that if gyros or inertial measurement devices are used to attempt to
|
|
correct for all error, the result can be not very robust, and include
|
|
many problematic assumptions. For this reason, we have found that
|
|
it is best to integrate this one simple gyro to simply improve robot odometry,
|
|
rather than a complicated and expensive IMU.
|
|
|
|
@ingroup OptionalClasses
|
|
|
|
*/
|
|
class ArAnalogGyro
|
|
{
|
|
public:
|
|
/// Constructor
|
|
AREXPORT ArAnalogGyro(ArRobot *robot);
|
|
/// Destructor
|
|
AREXPORT virtual ~ArAnalogGyro();
|
|
/// Gets if we really have a gyro or not
|
|
AREXPORT bool isActive(void) { return myIsActive; }
|
|
/// Lets the gyro correct readings
|
|
AREXPORT void activate(void);
|
|
/// Stops the gyro from correcting readings (still accumulates)
|
|
AREXPORT void deactivate(void);
|
|
/// If we have a gyro only mode
|
|
bool hasGyroOnlyMode(void) { return myHasGyroOnlyMode; }
|
|
/// If we're using gyro only mode
|
|
bool isGyroOnlyActive(void) { return myIsGyroOnlyActive; }
|
|
/// Activates it and puts it in gyro only mode
|
|
AREXPORT void activateGyroOnly(void);
|
|
/// If this class actually has data or not (if it has no data, the
|
|
/// robot is all there is)
|
|
bool hasNoInternalData(void) { return myHasNoData; }
|
|
/// Returns true if any amount of gyro data has yet been received, false if no readings have yet been received from the robot.
|
|
AREXPORT bool haveGottenData(void) { return myHaveGottenData; }
|
|
/// Gets a heading calculated from past gyro readings
|
|
AREXPORT double getHeading(void) const { return myHeading; }
|
|
/// Gets the temperature the gyro has
|
|
AREXPORT int getTemperature(void) const { return myTemperature; }
|
|
/// Set the parameters of the Kalman filter model
|
|
/**
|
|
@param gyroSigma the amount its off statically
|
|
|
|
@param inertialVar the proportional amount it is off
|
|
|
|
@param rotVar the amount the rotation is off proportionally
|
|
|
|
@param transVar the amount the translation throws off the heading
|
|
proportionally
|
|
**/
|
|
AREXPORT void setFilterModel(double gyroSigma, double inertialVar,
|
|
double rotVar, double transVar)
|
|
{ myGyroSigma = gyroSigma; myInertialVarianceModel = inertialVar;
|
|
myRotVarianceModel = rotVar; myTransVarianceModel = transVar; };
|
|
|
|
/// Returns the number of readings taken in the last second
|
|
AREXPORT int getPacCount(void) { return myPacCount; }
|
|
|
|
/// Gets the most recently calculated average rotational velocity (over one
|
|
//second)
|
|
AREXPORT double getAverage(void) const { return myLastAverage; }
|
|
/// Gets the time the last average was taken
|
|
AREXPORT ArTime getAverageTaken(void) const { return myLastAverageTaken; }
|
|
/// Gets the scaling factor used for multiplying the reading values received (default 1.626)
|
|
AREXPORT double getScalingFactor(void) const { return myScalingFactor; }
|
|
/// Sets the scaling factor used for multiplying the readings
|
|
AREXPORT void setScalingFactor(double factor) { myScalingFactor = factor; }
|
|
|
|
/// Internal packet handler for the gyro packets
|
|
AREXPORT bool handleGyroPacket(ArRobotPacket *packet);
|
|
/// internal function for correcting the encoder readings with the gyro data
|
|
AREXPORT double encoderCorrect(ArPoseWithTime deltaPose);
|
|
/// Internal connection callback; delays for a short amount of time to give the gyro enough time to stabilize before we try to use it
|
|
AREXPORT void stabilizingCallback(void);
|
|
/// Internal user task callback
|
|
AREXPORT void userTaskCallback(void);
|
|
/// Sets whether we log anomalies or not (temporary function for debugging)
|
|
void setLogAnomalies(bool logAnomalies) { myLogAnomalies = logAnomalies; }
|
|
protected:
|
|
// whether we're correcting readings or not
|
|
bool myIsActive;
|
|
// whether we're really getting readings or not
|
|
bool myHaveGottenData;
|
|
// our double for our scaling factor
|
|
double myScalingFactor;
|
|
// if we've gotten a reading this cycle
|
|
bool myReadingThisCycle;
|
|
// whether we're logging anomalies or not
|
|
bool myLogAnomalies;
|
|
// counting data
|
|
int myPacCount;
|
|
int myPacCurrentCount;
|
|
time_t myTimeLastPacket;
|
|
|
|
// data for averaging
|
|
ArTime myAverageStarted;
|
|
double myLastAverage;
|
|
ArTime myLastAverageTaken;
|
|
double myAverageTotal;
|
|
int myAverageCount;
|
|
|
|
|
|
// constants for kalman filtering
|
|
double myGyroSigma;
|
|
double myInertialVarianceModel;
|
|
double myRotVarianceModel; // deg2/deg
|
|
double myTransVarianceModel; // deg2/meter
|
|
|
|
// for if our gyro packets aren't aligned with our sips how much
|
|
// we've changed since we last got to correct one
|
|
double myAccumulatedDelta;
|
|
|
|
double myHeading;
|
|
int myTemperature;
|
|
double myLastHeading;
|
|
ArRobot *myRobot;
|
|
ArRetFunctor1C<bool, ArAnalogGyro, ArRobotPacket *> myHandleGyroPacketCB;
|
|
ArRetFunctor1C<double, ArAnalogGyro, ArPoseWithTime> myEncoderCorrectCB;
|
|
ArFunctorC<ArAnalogGyro> myStabilizingCB;
|
|
ArFunctorC<ArAnalogGyro> myUserTaskCB;
|
|
|
|
/// Gyro type
|
|
enum GyroType
|
|
{
|
|
GYRO_NONE, ///< No gyro
|
|
GYRO_ANALOG_COMPUTER, ///< Analog gyro used by computer
|
|
GYRO_ANALOG_CONTROLLER ///< Analog gyro used by microcontroller
|
|
};
|
|
GyroType myGyroType;
|
|
bool myHasNoData;
|
|
bool myHasGyroOnlyMode;
|
|
bool myIsGyroOnlyActive;
|
|
|
|
bool myGyroWorking;
|
|
};
|
|
|
|
#endif // ARANALOGGYRO_H
|
|
|
|
|