Commit b0df3886 authored by Timothy Stack's avatar Timothy Stack

Fix a silly race condition with the 'moving' flag. Add some comments

to the fault detection code.
parent 7cb8c70e
...@@ -7,6 +7,12 @@ ...@@ -7,6 +7,12 @@
#ifndef _faultDetection_hh #ifndef _faultDetection_hh
#define _faultDetection_hh #define _faultDetection_hh
/**
* @file faultDetection.hh
*
* Header file for classes used to detect and act on faults in the robot.
*/
#include <math.h> #include <math.h>
#include "garciaUtil.hh" #include "garciaUtil.hh"
...@@ -25,6 +31,9 @@ enum { ...@@ -25,6 +31,9 @@ enum {
FDF_VELOCITY_MISMATCH = (1L << FDB_VELOCITY_MISMATCH), FDF_VELOCITY_MISMATCH = (1L << FDB_VELOCITY_MISMATCH),
}; };
/**
* Interface for fault handlers.
*/
class faultCallback class faultCallback
{ {
...@@ -34,48 +43,139 @@ public: ...@@ -34,48 +43,139 @@ public:
* Destructor. * Destructor.
*/ */
virtual ~faultCallback(); virtual ~faultCallback();
/**
* Callback triggered when a fault is detected.
*
* @param faults Bitmask of FDF_ flags that specify the faults detected.
*/
virtual void faultDetected(unsigned long faults) = 0; virtual void faultDetected(unsigned long faults) = 0;
}; };
/**
* Fault detection class that periodically checks the telemetry for anything
* out of the ordinary.
*/
class faultDetection : public pollCallback class faultDetection : public pollCallback
{ {
public: public:
/**
* Maximum number of faults allowed before stopping the program. Usually a
* few faults will be detected during normal operation. So we only stop
* the world when things are really going wrong.
*/
static const float MAX_FAULTS = 8; static const float MAX_FAULTS = 8;
/**
* Desired threshold for the front ranger.
*/
static const float FRONT_RANGER_THRESHOLD = 0.360f; static const float FRONT_RANGER_THRESHOLD = 0.360f;
/**
* Desired threshold for the rear ranger.
*/
static const float REAR_RANGER_THRESHOLD = 0.217f; static const float REAR_RANGER_THRESHOLD = 0.217f;
/**
* Distance limit for when the robot should be idle. It isn't zero because
* there is still some play in the wheels while it's stopped.
*
* @see IDLE_VELOCITY_LIMIT
*/
static const float IDLE_DISTANCE_LIMIT = 0.013; static const float IDLE_DISTANCE_LIMIT = 0.013;
/**
* Velocity limit for when the robot should be idle.
*
* @see IDLE_DISTANCE_LIMIT
*/
static const float IDLE_VELOCITY_LIMIT = 0.013; static const float IDLE_VELOCITY_LIMIT = 0.013;
/**
* Construct a fault detector with the given arguments.
*
* @param garcia The garcia to monitor.
* @param db The dashboard to pull telemetry from.
*/
faultDetection(acpGarcia &garcia, dashboard *db); faultDetection(acpGarcia &garcia, dashboard *db);
/**
* Destructor.
*/
virtual ~faultDetection(); virtual ~faultDetection();
/**
* @param fc The callback to trigger when there is a fault.
*/
void setCallback(faultCallback *fc) { this->fd_callback = fc; }; void setCallback(faultCallback *fc) { this->fd_callback = fc; };
/**
* Set the distance limit, if the odometry value from the telemetry passes
* this value, a fault will be triggered.
*
* @param limit The distance limit in meters.
*/
void setDistanceLimit(float limit) void setDistanceLimit(float limit)
{ this->fd_distance_limit = fabsf(limit); }; { this->fd_distance_limit = fabsf(limit); };
/**
* @return The distance limit in meters.
*/
float getDistanceLimit() { return this->fd_distance_limit; }; float getDistanceLimit() { return this->fd_distance_limit; };
/**
* Set the velocity limit, if the odometry value from the telemetry passes
* this value, a fault will be triggered.
*
* @param limit The velocity limit in meters.
*/
void setVelocityLimit(float limit) void setVelocityLimit(float limit)
{ this->fd_velocity_limit = fabsf(limit); }; { this->fd_velocity_limit = fabsf(limit); };
float getVelocityLimit() { return this->fd_velocity_limit; };
/**
* @return The velocity limit in meters.
*/
float getVelocityLimit() { return this->fd_velocity_limit; };
/**
* Check the telemetry for any indications of a fault.
*
* @param now The current time in milliseconds.
*/
virtual bool update(unsigned long now); virtual bool update(unsigned long now);
private: private:
/**
* The garcia being monitored.
*/
acpGarcia &fd_garcia; acpGarcia &fd_garcia;
/**
* The dashboard to pull telemetry from.
*/
dashboard *fd_dashboard; dashboard *fd_dashboard;
/**
* The callback to trigger when there is a fault.
*/
faultCallback *fd_callback; faultCallback *fd_callback;
/**
* The distance limit.
*/
float fd_distance_limit; float fd_distance_limit;
/**
* The velocity limit.
*/
float fd_velocity_limit; float fd_velocity_limit;
/**
* The number of faults detected so far.
*/
unsigned int fd_fault_count; unsigned int fd_fault_count;
}; };
......
...@@ -312,6 +312,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback) ...@@ -312,6 +312,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
this->wm_garcia.queueBehavior(move); this->wm_garcia.queueBehavior(move);
move = NULL; move = NULL;
this->wm_moving = true;
} }
} }
...@@ -330,6 +331,7 @@ void wheelManager::setOrientation(float orientation, wmCallback *callback) ...@@ -330,6 +331,7 @@ void wheelManager::setOrientation(float orientation, wmCallback *callback)
else { else {
this->wm_garcia.queueBehavior(pivot); this->wm_garcia.queueBehavior(pivot);
pivot = NULL; pivot = NULL;
this->wm_moving = true;
} }
} }
...@@ -355,7 +357,6 @@ void wheelManager::motionStarted(acpObject *behavior) ...@@ -355,7 +357,6 @@ void wheelManager::motionStarted(acpObject *behavior)
distance = 1.0f; // XXX pivot, just assume a meter for now. distance = 1.0f; // XXX pivot, just assume a meter for now.
this->wm_dashboard->setDistanceLimit(distance * 1.1); this->wm_dashboard->setDistanceLimit(distance * 1.1);
this->wm_dashboard->setVelocityLimit(this->wm_speed * 1.1); this->wm_dashboard->setVelocityLimit(this->wm_speed * 1.1);
this->wm_moving = true;
if ((this->wm_last_status != aGARCIA_ERRFLAG_NORMAL) && if ((this->wm_last_status != aGARCIA_ERRFLAG_NORMAL) &&
(this->wm_last_status != aGARCIA_ERRFLAG_ABORT)) { (this->wm_last_status != aGARCIA_ERRFLAG_ABORT)) {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment