Commit 4fb9fd3f authored by Timothy Stack's avatar Timothy Stack

revert to older version

parent 48296462
......@@ -3,7 +3,7 @@
* Dan Flickinger
*
* 2004/11/16
* 2004/12/15
* 2004/11/19
*
*/
......
......@@ -10,9 +10,6 @@
#include "gcallbacks.h"
#define MIN_DISPLACEMENT 0.02f
#define MIN_ANGLE 0.02f
grobot::grobot() {
// default constructor
aIO_GetLibRef(&ioRef, &err);
......@@ -25,8 +22,6 @@ grobot::grobot() {
dright = 0.0f;
dt_init = 0.0f;
dt_final = 0.0f;
dx_est = 0.0f;
dy_est = 0.0f;
......@@ -138,13 +133,11 @@ void grobot::pbMove(float mdisplacement) {
mdisplacement = floor(mdisplacement);
mdisplacement = mdisplacement/100.0;
if (fabs(mdisplacement) > MIN_DISPLACEMENT) {
if (fabs(mdisplacement) > 0.05f) {
// send the move to the robot
// if you want to move less than MIN_DISPLACEMENT, FUCK OFF.
// if you want to move less than 5 cm, FUCK OFF.
std::cout << "Move length: " << mdisplacement << std::endl;
resetPosition();
acpValue moveLength((float)(mdisplacement));
pBehavior = garcia.createNamedBehavior("move", "move1");
pBehavior->setNamedValue("distance", &moveLength);
......@@ -201,13 +194,11 @@ void grobot::pbPivot(float pangle) {
}
}
if (fabs(pangle) > MIN_ANGLE) {
if (fabs(pangle) > 0.05f) {
// send the pivot to the robot
// If it wants to turn less than 0.05 radians, it gets the hose again
std::cout << "Pivot angle: " << pangle << std::endl;
resetPosition();
acpValue pivotAngle((float)(pangle));
pBehavior = garcia.createNamedBehavior("pivot", "pivot1");
pBehavior->setNamedValue("angle", &pivotAngle);
......@@ -220,14 +211,6 @@ void grobot::pbPivot(float pangle) {
// fake a successful pivot, and increment the callback counts
// if this is part of a goto sequence of commands
if (1 == gotolock) {
if (gotomexec == 0) {
// first pivot
dt_init = 0.0f;
} else {
// second pivot, or WHATEVER
dt_final = 0.0f;
}
++gotomexec;
++gotomcomplete;
}
......@@ -259,28 +242,16 @@ void grobot::dgoto(float Dx, float Dy, float Rf) {
gotop2 = 0;
// calculate rotation components
float Rir = atan2(Dy, Dx);
float Rfr = Rf - Rir;
dt_init = atan2(Dy, Dx); // dt_init is private to grobot
float Rfr = Rf - dt_init;
float moveL = sqrt((pow(Dx,2)) + (pow(Dy,2)));
if (0.0f == Rir && 0.0f == moveL && 0.0f == Rfr) {
if (0.0f == dt_init && 0.0f == moveL && 0.0f == Rfr) {
set_gotocomplete();
} else {
// execute primitives
dt_init = Rir;
dt_final = Rfr;
// reset position estimates
dx_est = 0.0f;
dy_est = 0.0f;
// create behaviors
pbPivot(Rir);
pbPivot(dt_init);
pbMove(moveL);
pbPivot(Rfr);
}
......@@ -305,6 +276,9 @@ void grobot::resetPosition() {
dleft = 0.0f;
dright = 0.0f;
dx_est = 0.0f;
dy_est = 0.0f;
}
......@@ -330,12 +304,11 @@ float grobot::getArclen() {
void grobot::getDisplacement(float &dxtemp, float &dytemp, float &dttemp) {
void grobot::getDisplacement(float &dxtemp, float &dytemp) {
// return the current displacement (for a goto move)
dxtemp = dx_est;
dytemp = dy_est;
dttemp = dt_est;
}
......@@ -419,38 +392,23 @@ void grobot::setCBstatus(int id, int stat, cb_type_t cbt) {
// a goto behavior is currently executing
++gotomcomplete;
if (cbt == CBT_MOVE && 2 == gotocomplete) {
if (cbt == CBT_MOVE) {
// get the Arclength, then estimate and store the displacement
// (the main move has completed)
// (assume that the first pivot was accurate)
float al_temp = getArclen();
dx_est = al_temp * cos(dt_init);
dy_est = al_temp * sin(dt_init);
gotop1 = stat;
}
if (cbt == CBT_PIVOT) {
// get the wheel odometry, then estimate and store the pivot angle
updatePosition();
//float pa_temp = (dright - dleft) / TRACK_WIDTH;
if (1 == gotomcomplete) {
if (1 == gotomcomplete) {
// first pivot has finished
// FIXME
//dt_init = pa_temp;
gotop1 = stat;
} else if (2 == gotomcomplete) {
// main move has executed
gotom1 = stat;
} else if (3 == gotomcomplete) {
} else if (3 == gotomcomplete) {
// second pivot has executed
// FIXME
//dt_final = pa_temp;
//dt_est = dt_init + dt_final;
gotop2 = stat;
// the goto command has completed
......@@ -459,10 +417,7 @@ void grobot::setCBstatus(int id, int stat, cb_type_t cbt) {
// set the gotocomplete flag
set_gotocomplete();
}
}
if (gotocomplete > 3) {
} else {
// weirdness
std::cout << "ERROR: Completion callback count "
<< "for goto command exceeded." << std::endl;
......
......@@ -51,7 +51,7 @@ class grobot {
void resetPosition();
void updatePosition();
float getArclen();
void getDisplacement(float &dxtemp, float &dytemp, float &dttemp);
void getDisplacement(float &dxtemp, float &dytemp);
int getGstatus();
int getGOTOstatus();
......@@ -72,19 +72,15 @@ class grobot {
// Wheel odometry values
float Vl; // left wheel velocity
float Vr; // right wheel velocity
float Vl; // left wheel velocity
float Vr; // right wheel velocity
float dleft; // left wheel distance
float dright; // right wheel distance
float dleft; // left wheel distance
float dright; // right wheel distance
float dt_init; // initial pivot angle for a goto command
float dt_final; // final pivot angle for a goto command
// position estimates after a goto command has finished:
float dx_est; // estimated displacement x
float dy_est; // estimated displacement y
float dt_est; // estimated orientation theta
float dt_init; // initial pivot angle for a goto command
float dx_est; // estimated displacement x
float dy_est; // estimated displacement y
......
......@@ -6,7 +6,7 @@
* Dan Flickinger
*
* 2004/11/12
* 2004/12/15
* 2004/12/09
*/
#include <stdio.h>
......@@ -63,10 +63,7 @@ static void handle_client_packet(grobot &bot,
case MTP_COMMAND_GOTO:
/* Record our robot id. */
robot_id = mp->data.command_goto->robot_id;
// theta should be calculated by grobot; override instead
theta = mp->data.command_goto->position.theta;
bot.dgoto(mp->data.command_goto->position.x,
mp->data.command_goto->position.y,
mp->data.command_goto->position.theta);
......@@ -79,10 +76,8 @@ static void handle_client_packet(grobot &bot,
bot.estop();
mup.robot_id = mp->data.command_stop->robot_id;
bot.getDisplacement(mup.position.x,
mup.position.y,
mup.position.theta);
bot.resetPosition(); // not really needed
bot.getDisplacement(mup.position.x, mup.position.y);
mup.position.theta = 0;
mup.status = MTP_POSITION_STATUS_IDLE;
if ((ump = mtp_make_packet(MTP_UPDATE_POSITION,
MTP_ROLE_RMC,
......@@ -269,17 +264,9 @@ int main(int argc, char *argv[])
}
mup.robot_id = robot_id;
bot.getDisplacement(mup.position.x,
mup.position.y,
mup.position.theta);
// override, for now:
mup.position.theta = theta;
// bot.resetPosition();
// No need to reset position, because it's done
// automatically at the start of each primitive
bot.getDisplacement(mup.position.x, mup.position.y);
bot.resetPosition();
mup.position.theta = theta;
if (rc < 0) {
mup.status = MTP_POSITION_STATUS_ERROR;
}
......
......@@ -5,7 +5,7 @@
* Dan Flickinger
*
* 2004/12/08
* 2004/12/15
* 2004/12/09
*/
......@@ -15,7 +15,7 @@
int main() {
float dx, dy, dr;
float dxe, dye, dte;
float dxe, dye;
int quitnow = 0;
int gstatus = 0;
......@@ -48,13 +48,12 @@ int main() {
// get the status
gstatus = mrrobot.getGOTOstatus();
mrrobot.getDisplacement(dxe, dye, dte);
mrrobot.getDisplacement(dxe, dye);
std::cout << "Goto move finished with status: " << gstatus
<< std::endl
<< "(Estimated position: " << dxe << ", "
<< dye << "angle: "
<< dte << ".)" << std::endl;
<< dye << ".)" << std::endl;
......
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