Commit 8def3310 authored by Timothy Stack's avatar Timothy Stack
Browse files

Start the null command explicitly which is needed for dynamic obstacle

avoidance to work correctly.
parent ce719f37
......@@ -150,6 +150,7 @@ pilotClient::~pilotClient()
bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
{
struct mtp_command_startnull *mcsn;
struct mtp_command_goto *mcg;
struct mtp_command_stop *mcs;
struct mtp_command_wheels *mcw;
......@@ -290,33 +291,36 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
}
break;
/* DAN */
case MTP_COMMAND_STARTNULL:
if (this->pc_role == MTP_ROLE_RMC) {
mcsn = &mp->data.mtp_payload_u.command_startnull;
/* start NULL command */
this->pc_wheel_manager.
startNULL(mcsn->acceleration,
new pilotMoveCallback(notify_list,
mcsn->command_id, 0.0f));
retval = true;
}
break;
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(DEFAULT_ROBOT_ACCELERATION,
new pilotMoveCallback(notify_list,
mcw->command_id, 0.0f));
this->pc_wheel_manager.setWheels(mcw->vleft, mcw->vright);
retval = true;
}
break;
case MTP_REQUEST_REPORT:
{
struct mtp_garcia_telemetry *mgt;
......
......@@ -372,7 +372,12 @@ void wheelManager::startNULL(float accel, wmCallback *callback) {
acpValue av;
acpObject *nullb = NULL;
av.set(FRONT_RANGER_THRESHOLDS[THRESH_HIGH]);
this->wm_garcia.setNamedValue("front-ranger-threshold", &av);
av.set(REAR_RANGER_THRESHOLDS[THRESH_HIGH]);
this->wm_garcia.setNamedValue("rear-ranger-threshold", &av);
nullb = this->wm_garcia.createNamedBehavior("null", NULL);
av.set((float)(accel)); /* Do I really need to force it to a float? */
......@@ -384,23 +389,15 @@ void wheelManager::startNULL(float accel, wmCallback *callback) {
av.set(new endCallback(*this, nullb, callback));
nullb->setNamedValue("completion-callback", &av);
if (nullb == NULL && callback != NULL) {
callback->call(aGARCIA_ERRFLAG_WONTEXECUTE, 0);
delete callback;
callback = NULL;
}
else {
this->wm_garcia.queueBehavior(nullb);
nullb = NULL;
}
this->wm_garcia.queueBehavior(nullb);
nullb = NULL;
this->wm_moving = true;
}
}
void wheelManager::setWheels(float vl, float vr) {
acpValue av_L;
......@@ -408,6 +405,9 @@ void wheelManager::setWheels(float vl, float vr) {
float maxspeed = 0.0f;
if (!this->wm_moving)
return;
if (fabsf(vl) >= fabsf(vr)) {
maxspeed = fabsf(vl) * 100.0;
}
......@@ -442,13 +442,10 @@ void wheelManager::setWheels(float vl, 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;
}
......
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