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