Commit 1a904fa6 authored by Timothy Stack's avatar Timothy Stack

Add a contact point to the contact-report when the robot stalls

parent 2a8068fd
......@@ -55,6 +55,8 @@ void pilotMoveCallback::call(int status)
mtp_packet_t mp;
mtp_status_t ms;
fprintf(stderr, "callback: %d\n", status);
switch (status) {
case aGARCIA_ERRFLAG_NORMAL:
ms = MTP_POSITION_STATUS_COMPLETE;
......@@ -316,6 +318,11 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
points[count].y = sin(-M_PI + 0.40) * mgt->rear_ranger_right;
count += 1;
}
else if (mgt->stall_contact && count == 0) {
points[count].x = (mgt->stall_contact < 0) ? -0.20 : 0.15;
points[count].y = 0;
count += 1;
}
mtp_init_packet(&cmp,
MA_Opcode, MTP_CONTACT_REPORT,
......
......@@ -149,7 +149,9 @@ aErr endCallback::call()
status = this->ec_behavior->
getNamedValue("completion-status")->getIntVal();
this->ec_wheel_manager.motionFinished(status, this->ec_callback);
this->ec_wheel_manager.motionFinished(this->ec_behavior,
status,
this->ec_callback);
this->ec_callback = NULL;
......@@ -321,11 +323,23 @@ void wheelManager::motionStarted(void)
this->wm_dashboard->startMove(this->wm_garcia);
}
void wheelManager::motionFinished(int status, wmCallback *callback)
void wheelManager::motionFinished(acpObject *behavior,
int status,
wmCallback *callback)
{
if (debug) {
fprintf(stderr, "debug: motion finished -- %d\n", status);
}
if (status == aGARCIA_ERRFLAG_STALL) {
float distance = behavior->getNamedValue("distance")->getFloatVal();
this->wm_dashboard->getTelemetry()->stall_contact =
(distance < 0) ? -1 : 1;
}
else {
this->wm_dashboard->getTelemetry()->stall_contact = 0;
}
if (status != aGARCIA_ERRFLAG_WONTEXECUTE) {
if ((status != aGARCIA_ERRFLAG_NORMAL) &&
......
......@@ -161,7 +161,7 @@ public:
* values.)
* @param callback The callback to trigger.
*/
void motionFinished(int status, wmCallback *callback);
void motionFinished(acpObject *behavior, int status, wmCallback *callback);
/**
* Check the object to make sure it is internally consistent.
......
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