All new accounts created on Gitlab now require administrator approval. If you invite any collaborators, please let Flux staff know so they can approve the accounts.

Commit eb680d4f authored by Daniel Flickinger's avatar Daniel Flickinger

A new posture regulator to play with, plus some logging facilities for pilot.

To use the 'B' posture regulator, define USE_POSTREG_B in masterController.c
parent adc4f7c9
This diff is collapsed.
......@@ -18,7 +18,7 @@
#include "garciaUtil.hh"
#include "pilotClient.hh"
#define DEFAULT_ROBOT_ACCELERATION 0.4f
/**
* Callback passed to the wheelManager when performing movements.
......@@ -108,7 +108,7 @@ void pilotMoveCallback::call(int status, float odometer)
i != this->pmc_notify_list.end();
i++) {
pilotClient *pc = *i;
mtp_send_packet(pc->getHandle(), &mp);
}
}
......@@ -134,7 +134,7 @@ pilotClient::~pilotClient()
if (debug) {
fprintf(stderr, "debug: rmc client disconnected\n");
}
this->pc_wheel_manager.stop();
pc_rmc_client = NULL;
}
......@@ -176,9 +176,9 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
"debug: rmc client connected - %s\n",
mp->data.mtp_payload_u.init.msg);
}
pilotClient::pc_rmc_client = this;
retval = true;
}
break;
......@@ -188,33 +188,33 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
"debug: emulab client connected - %s\n",
mp->data.mtp_payload_u.init.msg);
}
retval = true;
break;
}
break;
case MTP_COMMAND_GOTO:
if (this->pc_role == MTP_ROLE_RMC) {
pilotClient::iterator i;
struct mtp_packet cmp;
mcg = &mp->data.mtp_payload_u.command_goto;
cmp = *mp;
cmp.role = MTP_ROLE_ROBOT;
for (i = notify_list.begin(); i != notify_list.end(); i++) {
pilotClient *pc = *i;
if (pc->getRole() == MTP_ROLE_EMULAB) {
mtp_send_packet(pc->getHandle(), &cmp);
}
}
if ((mcg->position.x != 0.0) || (mcg->position.y != 0.0)) {
acpValue av;
float theta;
if (debug) {
fprintf(stderr,
"debug: move to %f %f\n",
......@@ -239,7 +239,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
"debug: reorient to %f\n",
mcg->position.theta);
}
this->pc_wheel_manager.setSpeed(mcg->speed);
this->pc_wheel_manager.setOrientation(
mcg->position.theta,
......@@ -247,7 +247,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
mcg->command_id,
mcg->position.theta));
}
retval = true;
}
break;
......@@ -257,13 +257,13 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
struct mtp_packet cmp, rmp;
pilotClient::iterator i;
bool send_idle = false;
mcs = &mp->data.mtp_payload_u.command_stop;
if (debug) {
fprintf(stderr, "debug: full stop\n");
}
if (!this->pc_wheel_manager.stop()) {
mtp_init_packet(&rmp,
MA_Opcode, MTP_UPDATE_POSITION,
......@@ -285,38 +285,38 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
if (send_idle)
mtp_send_packet(pc->getHandle(), &rmp);
}
retval = true;
}
break;
/* DAN */
case MTP_COMMAND_WHEELS:
if (this->pc_role == MTP_ROLE_RMC) {
mcw = &mp->data.mtp_payload_u.command_wheels;
if (debug > 1) {
fprintf(stderr,
"debug: set wheels to: %f %f\n",
mcw->vleft,
mcw->vright);
}
/* start NULL command (if needed) */
/* FIXME: need to configure acceleration here */
/* I don't care about command_id or theta! */
this->pc_wheel_manager.startNULL(0.2f,
this->pc_wheel_manager.startNULL(DEFAULT_ROBOT_ACCELERATION,
new pilotMoveCallback(notify_list,
mcw->command_id, 0.0f));
this->pc_wheel_manager.setWheels(mcw->vleft, mcw->vright);
retval = true;
}
break;
}
break;
case MTP_REQUEST_REPORT:
{
struct mtp_garcia_telemetry *mgt;
......@@ -324,7 +324,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
pilotClient::iterator i;
struct mtp_packet cmp;
int count = 0;
mgt = this->pc_dashboard.getTelemetry();
if ((mgt->front_ranger_left != 0.0) &&
......@@ -355,13 +355,13 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
points[count].y = sin(M_PI_2) * mgt->side_ranger_left;
count += 1;
}
if (mgt->side_ranger_right != 0.0) {
points[count].x = cos(-M_PI_2) * mgt->side_ranger_right;
points[count].y = sin(-M_PI_2) * mgt->side_ranger_right;
count += 1;
}
if ((mgt->rear_ranger_left != 0.0) &&
(mgt->rear_ranger_right != 0.0)) {
float min_range;
......@@ -396,13 +396,13 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
MA_ContactPointCount, count,
MA_ContactPoints, points,
MA_TAG_DONE);
for (i = notify_list.begin(); i != notify_list.end(); i++) {
pilotClient *pc = *i;
mtp_send_packet(pc->getHandle(), &cmp);
}
retval = true;
}
break;
......@@ -411,6 +411,6 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
fprintf(stderr, "warning: unhandled opcode - %d\n", mp->data.opcode);
break;
}
return retval;
}
......@@ -57,7 +57,7 @@ public:
* Method called when the behavior starts.
*/
aErr call();
private:
/**
......@@ -69,7 +69,7 @@ private:
* The behavior this callback is attached to.
*/
acpObject *sc_behavior;
};
/**
......@@ -88,7 +88,7 @@ public:
* @param callback The wheelManager callback that should be triggered.
*/
endCallback(wheelManager &wm, acpObject *behavior, wmCallback *callback);
/**
* Destructor.
*/
......@@ -98,24 +98,24 @@ public:
* Method called when the behavior finishes.
*/
aErr call();
private:
/**
* The wheelManager to notify when some motion has finished.
*/
wheelManager &ec_wheel_manager;
/**
* The behavior this callback is attached to.
*/
acpObject *ec_behavior;
/**
* The wheelManager callback that should be triggered.
*/
wmCallback *ec_callback;
};
wmCallback::~wmCallback()
......@@ -135,7 +135,7 @@ startCallback::~startCallback()
aErr startCallback::call()
{
this->sc_wheel_manager.motionStarted(this->sc_behavior);
return aErrNone;
}
......@@ -156,7 +156,7 @@ endCallback::~endCallback()
aErr endCallback::call()
{
int status;
status = this->ec_behavior->
getNamedValue("completion-status")->getIntVal();
......@@ -165,7 +165,11 @@ aErr endCallback::call()
this->ec_callback);
this->ec_callback = NULL;
// if (debug) {
// printf("COMPLETION CALLBACK\n");
// }
return aErrNone;
}
......@@ -186,10 +190,10 @@ wheelManager::~wheelManager()
void wheelManager::setSpeed(float speed)
{
acpValue av;
if ((speed < MINIMUM_SPEED) || (speed > MAXIMUM_SPEED))
speed = DEFAULT_SPEED;
this->wm_speed = speed;
av.set(speed);
this->wm_garcia.setNamedValue("speed", &av);
......@@ -198,7 +202,7 @@ void wheelManager::setSpeed(float speed)
acpObject *wheelManager::createPivot(float angle, wmCallback *callback)
{
acpObject *retval = NULL;
assert(this->invariant());
/* First, reduce to a single rotation, */
......@@ -228,11 +232,11 @@ acpObject *wheelManager::createPivot(float angle, wmCallback *callback)
av.set(new startCallback(*this, retval));
retval->setNamedValue("execute-callback", &av);
av.set(new endCallback(*this, retval, callback));
retval->setNamedValue("completion-callback", &av);
}
return retval;
}
......@@ -257,11 +261,11 @@ acpObject *wheelManager::createMove(float distance, wmCallback *callback)
av.set(new startCallback(*this, retval));
retval->setNamedValue("execute-callback", &av);
av.set(new endCallback(*this, retval, callback));
retval->setNamedValue("completion-callback", &av);
}
return retval;
}
......@@ -296,7 +300,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
this->wm_garcia.setNamedValue("front-ranger-threshold", &av);
av.set(REAR_RANGER_THRESHOLDS[tl]);
this->wm_garcia.setNamedValue("rear-ranger-threshold", &av);
mgt = this->wm_dashboard->getTelemetry();
diff = fabsf(mgt->rear_ranger_left - mgt->rear_ranger_right);
if (diff > 0.08f) {
......@@ -319,7 +323,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
/* Skipping everything. */
if (callback != NULL) {
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE, 0);
delete callback;
callback = NULL;
}
......@@ -332,7 +336,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
this->wm_garcia.queueBehavior(pivot);
pivot = NULL;
}
this->wm_garcia.queueBehavior(move);
move = NULL;
this->wm_moving = true;
......@@ -346,7 +350,7 @@ void wheelManager::setOrientation(float orientation, wmCallback *callback)
if ((pivot = this->createPivot(orientation, callback)) == NULL) {
if (callback != NULL) {
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE, 0);
delete callback;
callback = NULL;
}
......@@ -359,19 +363,18 @@ void wheelManager::setOrientation(float orientation, wmCallback *callback)
}
/* DAN */
void wheelManager::startNULL(float accel, wmCallback *callback) {
/* Start the NULL primitive only if the garcia is idle */
assert(this->invariant());
if (this->wm_garcia.getNamedValue("idle")->getBoolVal()) {
acpValue av;
acpObject *nullb = NULL;
nullb = this->wm_garcia.createNamedBehavior("null", NULL);
av.set((float)(accel)); /* Do I really need to force it to a float? */
nullb->setNamedValue("acceleration", &av);
......@@ -383,69 +386,69 @@ void wheelManager::startNULL(float accel, wmCallback *callback) {
if (nullb == NULL && callback != NULL) {
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE, 0);
delete callback;
callback = NULL;
}
else {
this->wm_garcia.queueBehavior(nullb);
this->wm_garcia.queueBehavior(nullb);
nullb = NULL;
}
}
}
void wheelManager::setWheels(float vl, float vr) {
acpValue av_L;
acpValue av_R;
float maxspeed = 0.0f;
if (fabsf(vl) >= fabsf(vr)) {
maxspeed = fabsf(vl) * 100.0;
}
else {
maxspeed = fabsf(vr) * 100.0;
}
if (fabsf(vl) < 0.01) {
vl = 0.0f;
}
if (fabsf(vr) < 0.01) {
vr = 0.0f;
}
if (vl > MAX_WHEELSPEED) {
vl = MAX_WHEELSPEED;
}
if (vl < -MAX_WHEELSPEED) {
vl = -MAX_WHEELSPEED;
}
if (vr > MAX_WHEELSPEED) {
vr = MAX_WHEELSPEED;
}
if (vr < -MAX_WHEELSPEED) {
vr = -MAX_WHEELSPEED;
}
av_L.set((float)(vl));
av_R.set((float)(vr));
this->wm_garcia.setNamedValue("damped-speed-left", &av_L);
this->wm_garcia.setNamedValue("damped-speed-right", &av_R);
/* handle fault detection */
this->wm_dashboard->setDistanceLimit(10000.0f);
this->wm_dashboard->setVelocityLimit(maxspeed);
this->wm_moving = true;
}
......@@ -453,18 +456,18 @@ void wheelManager::setWheels(float vl, float vr) {
bool wheelManager::stop(void)
{
acpValue av;
av.set((float)(0.0));
this->wm_garcia.setNamedValue("damped-speed-left", &av);
this->wm_garcia.setNamedValue("damped-speed-right", &av);
this->wm_garcia.flushQueuedBehaviors();
if (debug) {
fprintf(stderr, "debug: STOP WHEELS\n");
}
return this->wm_moving;
}
......@@ -472,7 +475,7 @@ void wheelManager::motionStarted(acpObject *behavior)
{
float distance;
acpValue *av;
if (debug) {
fprintf(stderr, "debug: motion started\n");
}
......@@ -489,7 +492,7 @@ void wheelManager::motionStarted(acpObject *behavior)
if (debug) {
fprintf(stderr, "debug: clear error LED\n");
}
this->wm_dashboard->remUserLEDClient(&this->wm_error_notice);
this->wm_last_status = 0;
}
......@@ -503,33 +506,37 @@ void wheelManager::motionFinished(acpObject *behavior,
wmCallback *callback)
{
float odometer = 0.0f;
if (debug) {
fprintf(stderr, "debug: motion finished -- %d\n", status);
fprintf(stderr, "debug: motion finished with status %d\n", status);
}
if (status == aGARCIA_ERRFLAG_STALL) {
float distance = behavior->getNamedValue("distance")->getFloatVal();
if (debug) {
fprintf(stderr, "debug: STALL at %f\n", distance);
}
this->wm_dashboard->getTelemetry()->stall_contact =
(distance < 0) ? -1 : 1;
}
else {
this->wm_dashboard->getTelemetry()->stall_contact = 0;
}
if (status != aGARCIA_ERRFLAG_WONTEXECUTE) {
float left_odometer, right_odometer;
if ((status != aGARCIA_ERRFLAG_NORMAL) &&
(status != aGARCIA_ERRFLAG_ABORT)) {
if (debug) {
fprintf(stderr, "debug: set error LED\n");
}
this->wm_dashboard->addUserLEDClient(&this->wm_error_notice);
}
this->wm_dashboard->endMove(left_odometer, right_odometer);
if ((left_odometer / fabsf(left_odometer)) ==
(right_odometer / fabsf(right_odometer))) {
......@@ -539,20 +546,20 @@ void wheelManager::motionFinished(acpObject *behavior,
right_odometer / right_odometer);
odometer = left_odometer;
}
this->wm_dashboard->remUserLEDClient(&this->wm_moving_notice);
this->wm_last_status = status;
}
this->wm_dashboard->setVelocityLimit(faultDetection::IDLE_VELOCITY_LIMIT);
this->wm_dashboard->setDistanceLimit(faultDetection::IDLE_DISTANCE_LIMIT);
if (callback != NULL) {
this->wm_moving = false; // XXX This assumes callbacks on the last move
callback->call(this->wm_last_status, odometer);
delete callback;
callback = NULL;
}
......
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