Commit 234c0e19 authored by Timothy Stack's avatar Timothy Stack

Comments

parent 072a1fd7
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* Copyright (c) 2005, 2006 University of Utah and the Flux Group.
* All rights reserved.
*/
......@@ -212,6 +212,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
}
}
/* Figure out if they want us to do a move or a pivot. */
if ((mcg->position.x != 0.0) || (mcg->position.y != 0.0)) {
acpValue av;
float theta;
......@@ -331,6 +332,7 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
mgt = this->pc_dashboard.getTelemetry();
/* Compute "contacts" based on the sensor reports. */
if ((mgt->front_ranger_left != 0.0) &&
(mgt->front_ranger_right != 0.0)) {
float min_range;
......@@ -344,6 +346,10 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
count += 1;
}
else if (mgt->front_ranger_left != 0.0) {
/*
* The front sensors are angled out a bit, the 0.40 angle seems
* to work well enough.
*/
points[count].x = cos(0.40) * mgt->front_ranger_left;
points[count].y = sin(0.40) * mgt->front_ranger_left;
count += 1;
......@@ -379,6 +385,13 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
count += 1;
}
else if (mgt->rear_ranger_left != 0.0) {
/*
* XXX This computation is probably wrong... We should have a
* constant offset for Y and set X to be the ranger value plus
* some constant. I don't remember exactly what the ranger
* value means for these (i.e. if it is based off the center
* of the robot or not).
*/
points[count].x = cos(M_PI - 0.40) * mgt->rear_ranger_left;
points[count].y = sin(M_PI - 0.40) * mgt->rear_ranger_left;
count += 1;
......
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* Copyright (c) 2005, 2006 University of Utah and the Flux Group.
* All rights reserved.
*/
......@@ -292,6 +292,7 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
distance = -distance;
}
/* For small distances, drop the sensor thresholds so we can maneuver. */
if (distance <= 0.60f)
tl = THRESH_LOW;
else
......@@ -303,7 +304,16 @@ void wheelManager::setDestination(float x, float y, wmCallback *callback)
mgt = this->wm_dashboard->getTelemetry();
diff = fabsf(mgt->rear_ranger_left - mgt->rear_ranger_right);
/*
* If the rear rangers detect an obstacle that is not perpendicular to the
* robot, we might need to pivot in the opposite direction so the tail
* doesn't hit the obstacle.
*/
if (diff > 0.08f) {
/*
* Make sure the direction we want to pivot is clear, otherwise, pivot
* the opposite direction.
*/
if ((mgt->rear_ranger_right == 0.0f) ||
(mgt->rear_ranger_left < mgt->rear_ranger_right)) {
if (angle < 0.0f) {
......@@ -535,6 +545,10 @@ void wheelManager::motionFinished(acpObject *behavior,
}
this->wm_dashboard->endMove(left_odometer, right_odometer);
/*
* If both wheels moved the same direction it was a straight move,
* otherwise, it was a pivot.
*/
if ((left_odometer / fabsf(left_odometer)) ==
(right_odometer / fabsf(right_odometer))) {
printf(" %f %f -- %f %f\n",
......
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