Commit 36c84144 authored by Daniel Flickinger's avatar Daniel Flickinger
Browse files

moved the zero length move/pivot error checking and callback incrementing into...

moved the zero length move/pivot error checking and callback incrementing into the primitive functions themselves, instead of the dgoto method.

gorobot has been messing up anytime a goto command sequence results in a zero length pivot. (Which means the robot will fuckup and act stupid any time you try to get it to move in a straight line!)

Things would be much easier if the Garcia could simply execute zero length commands without barfing.
parent 4cd61460
...@@ -3,7 +3,7 @@ ...@@ -3,7 +3,7 @@
* Dan Flickinger * Dan Flickinger
* *
* 2004/11/16 * 2004/11/16
* 2004/12/09 * 2004/12/15
*/ */
#include "grobot.h" #include "grobot.h"
...@@ -128,11 +128,25 @@ void grobot::setvPath(float Wv, float Wr) { ...@@ -128,11 +128,25 @@ void grobot::setvPath(float Wv, float Wr) {
void grobot::pbMove(float mdisplacement) { void grobot::pbMove(float mdisplacement) {
// execute a move primitive // execute a move primitive
acpValue moveLength((float)(mdisplacement)); if (mdisplacement != 0.0f) {
pBehavior = garcia.createNamedBehavior("move", "move1"); acpValue moveLength((float)(mdisplacement));
pBehavior->setNamedValue("distance", &moveLength); pBehavior = garcia.createNamedBehavior("move", "move1");
pBehavior->setNamedValue("distance", &moveLength);
createPRIMbehavior(CBT_MOVE); createPRIMbehavior(CBT_MOVE);
} else {
// a zero length move will fuckup the robot!
std::cout << "ZERO LENGTH MOVE" << std::endl;
// fake a successful move, and increment the callback counts
// if this is part of a goto sequence of commands
if (1 == gotolock) {
++gotomexec;
++gotomcomplete;
dx_est = 0.0f; // move nowhere!
dy_est = 0.0f; // move nowhere!
}
}
} }
...@@ -166,12 +180,24 @@ void grobot::pbPivot(float pangle) { ...@@ -166,12 +180,24 @@ void grobot::pbPivot(float pangle) {
} }
} }
if (pangle != 0.0f) {
acpValue pivotAngle((float)(pangle));
pBehavior = garcia.createNamedBehavior("pivot", "pivot1");
pBehavior->setNamedValue("angle", &pivotAngle);
acpValue pivotAngle((float)(pangle)); createPRIMbehavior(CBT_PIVOT);
pBehavior = garcia.createNamedBehavior("pivot", "pivot1"); } else {
pBehavior->setNamedValue("angle", &pivotAngle); // a zero angle pivot will fuckup the robot!
std::cout << "ZERO ANGLE PIVOT" << std::endl;
createPRIMbehavior(CBT_PIVOT);
// fake a successful pivot, and increment the callback counts
// if this is part of a goto sequence of commands
if (1 == gotolock) {
++gotomexec;
++gotomcomplete;
}
}
} }
...@@ -200,42 +226,18 @@ void grobot::dgoto(float Dx, float Dy, float Rf) { ...@@ -200,42 +226,18 @@ void grobot::dgoto(float Dx, float Dy, float Rf) {
// calculate rotation components // calculate rotation components
dt_init = atan2(Dy, Dx); // dt_init is private to grobot dt_init = atan2(Dy, Dx); // dt_init is private to grobot
float Rfr = Rf - dt_init; float Rfr = Rf - dt_init;
float moveL = sqrt((pow(Dx,2)) + (pow(Dy,2))); float moveL = sqrt((pow(Dx,2)) + (pow(Dy,2)));
// execute primitives (a zero length pivot or move will BARF!!!) if (0.0f == dt_init && 0.0f == moveL && 0.0f == Rfr) {
if (0.0f != dt_init) { set_gotocomplete();
pbPivot(dt_init);
} else { } else {
// fake pivot // execute primitives
++gotomexec;
++gotomcomplete; pbPivot(dt_init);
gotop1 = 0;
}
if (0.0f != moveL) {
pbMove(moveL); pbMove(moveL);
} else {
// fake move
++gotomexec;
++gotomcomplete;
gotom1 = 0;
dx_est = 0.0f; // move nowhere!
dy_est = 0.0f; // move nowhere!
}
if (0.0f != Rfr) {
pbPivot(Rfr); pbPivot(Rfr);
} else {
// fake pivot
++gotomexec;
++gotomcomplete;
gotop2 = 0;
} }
if (0.0f == dt_init && 0.0f == moveL && 0.0f == Rfr)
set_gotocomplete();
} else { } else {
// if a goto is already executing, drop the command // if a goto is already executing, drop the command
......
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