From 4d7fa593fbedeb3d6ee18f0fb828492bcff81d9f Mon Sep 17 00:00:00 2001
From: Daniel Flickinger <flikx@flux.utah.edu>
Date: Wed, 15 Dec 2004 22:35:18 +0000
Subject: [PATCH] Reverted the previous behavior of goto commands. Estimated
positions for pivots are not calculated, they are assumed.
---
robots/primotion/dgrobot/grobot.cc | 21 +++++++++++++--------
1 file changed, 13 insertions(+), 8 deletions(-)
diff --git a/robots/primotion/dgrobot/grobot.cc b/robots/primotion/dgrobot/grobot.cc
index 5723dfaf0f..716519d14b 100755
--- a/robots/primotion/dgrobot/grobot.cc
+++ b/robots/primotion/dgrobot/grobot.cc
@@ -260,17 +260,20 @@ void grobot::dgoto(float Dx, float Dy, float Rf) {
// calculate rotation components
float Rir = atan2(Dy, Dx);
- float Rfr = Rf - dt_init;
+ float Rfr = Rf - Rir;
float moveL = sqrt((pow(Dx,2)) + (pow(Dy,2)));
- if (0.0f == dt_init && 0.0f == moveL && 0.0f == Rfr) {
+ if (0.0f == Rir && 0.0f == moveL && 0.0f == Rfr) {
set_gotocomplete();
} else {
// execute primitives
+ dt_init = Rir;
+ dt_final = Rfr;
+
// reset position estimates
- dt_init = 0.0f;
- dt_final = 0.0f;
+
+
dx_est = 0.0f;
dy_est = 0.0f;
@@ -432,19 +435,21 @@ void grobot::setCBstatus(int id, int stat, cb_type_t cbt) {
updatePosition();
- float pa_temp = (dright - dleft) / TRACK_WIDTH;
+ //float pa_temp = (dright - dleft) / TRACK_WIDTH;
if (1 == gotomcomplete) {
// first pivot has finished
- dt_init = pa_temp;
+ // FIXME
+ //dt_init = pa_temp;
gotop1 = stat;
} else if (3 == gotomcomplete) {
// second pivot has executed
- dt_final = pa_temp;
- dt_est = dt_init + dt_final;
+ // FIXME
+ //dt_final = pa_temp;
+ //dt_est = dt_init + dt_final;
gotop2 = stat;
--
GitLab