libnxter  0.1
Odometer2.nxc
Go to the documentation of this file.
1 /* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
2 /*
3  Odometer2.nxc
4  Copyright (C) 2008 Naba Kumar <naba@gnome.org>
5 
6  This program is free software; you can redistribute it and/or modify
7  it under the terms of the GNU General Public License as published by
8  the Free Software Foundation; either version 2 of the License, or
9  (at your option) any later version.
10 
11  This program is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU General Public License for more details.
15 
16  You should have received a copy of the GNU General Public License
17  along with this program; if not, write to the Free Software
18  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19 */
20 
21 #ifndef _ODOMETER2_H_
22 #define _ODOMETER2_H_
23 
35 //#define ENABLE_DEBUG
36 //#define ENABLE_TEST
37 #include "Debug.nxc"
38 
39 #include "Vector.nxc"
40 #include "Map.nxc"
41 
42 #define INFINITY 2147483647 /* 2^31 -1 */
43 
48 struct Odometer2
49 {
50  /* Wheel anatomy */
53  int gearRatio;
57  /* Accumulative position, time and samles count since the begining */
58  Vector cumulativePosition;
59  long cumulativeTime;
60  long cumulativeSamplesCount;
61 
62  /* Accumulative position, time and samles count since the delta clear */
63  bool enableDeltaPosition;
64  Vector deltaPosition;
65  long deltaTime;
66  long deltaSamplesCount;
67 
68  /* Last sample: dx, position moved in last sample and dt sample period */
69  bool enableDPosition;
70  Vector dPosition;
71  long dTime;
72 
73  /* Speed of movement */
74  long speed;
75 
76  /* Last states used for internal computation */
77  long lastTachoLeft;
78  long lastTachoRight;
79  long lastSampleTime;
80 };
81 
93 safecall void Odometer2Step(Odometer2 &wheelBase)
94 {
95  long tachoLeft = GetOutput(wheelBase.leftWheelMotor, RotationCount);
96  long tachoRight = GetOutput(wheelBase.rightWheelMotor, RotationCount);
97  long dLeftTacho = (tachoLeft - wheelBase.lastTachoLeft);
98  long dRightTacho = (tachoRight - wheelBase.lastTachoRight);
99  long tick = CurrentTick();
100 
101  /* The actual v is also supposed to be divided by 22918.31 as per
102  * the equation, but we will defer it for more approriate time to
103  * preserve integer precision.
104  */
105  long v = (((dLeftTacho + dRightTacho) *
106  wheelBase.wheelDiameter) * wheelBase.gearRatio);
107  long w = ((dRightTacho - dLeftTacho) * wheelBase.wheelDiameter *
108  wheelBase.gearRatio)/(wheelBase.axialLength * 200);
109 
110  /* dx odometry */
111  if (wheelBase.enableDPosition)
112  {
113  wheelBase.dPosition.theta = w;
114  wheelBase.dPosition.x = (v * Cos(w))/2291831;
115  wheelBase.dPosition.y = (v * Sin(w))/2291831;
116  wheelBase.dTime = tick - wheelBase.lastSampleTime;
117  }
118 
119  /* Delta odometry */
120  if (wheelBase.enableDeltaPosition)
121  {
122  wheelBase.deltaPosition.theta = wheelBase.deltaPosition.theta + w;
123  if (wheelBase.deltaPosition.theta >= 360)
124  wheelBase.deltaPosition.theta %= 360;
125  wheelBase.deltaPosition.x =
126  wheelBase.deltaPosition.x +
127  ((v * Cos(wheelBase.deltaPosition.theta))/2291831);
128  wheelBase.deltaPosition.y =
129  wheelBase.deltaPosition.y +
130  ((v * Sin(wheelBase.deltaPosition.theta))/2291831);
131  wheelBase.deltaTime += wheelBase.dTime;
132  wheelBase.deltaSamplesCount++;
133  }
134 
135  /* Cumulative odometry */
136  wheelBase.cumulativePosition.theta =
137  wheelBase.cumulativePosition.theta + w;
138  if (wheelBase.cumulativePosition.theta >= 360)
139  wheelBase.cumulativePosition.theta %= 360;
140  if (wheelBase.cumulativePosition.theta < 0)
141  wheelBase.cumulativePosition.theta += 360;
142  wheelBase.cumulativePosition.x =
143  wheelBase.cumulativePosition.x +
144  ((v * Cos(wheelBase.cumulativePosition.theta))/2291831);
145  wheelBase.cumulativePosition.y =
146  wheelBase.cumulativePosition.y +
147  ((v * Sin(wheelBase.cumulativePosition.theta))/2291831);
148  wheelBase.cumulativeTime += wheelBase.dTime;
149  wheelBase.cumulativeSamplesCount++;
150 
151  /* Preserve past state */
152  wheelBase.lastTachoLeft = tachoLeft;
153  wheelBase.lastTachoRight = tachoRight;
154  wheelBase.lastSampleTime = tick;
155  wheelBase.speed = (v * 1000) / wheelBase.dTime; /* 1s = 1000ms */
156 }
157 
170 void Odometer2Init(Odometer2 &wheelBase,
171  int leftWheelMotor, int rightWheelMotor,
172  int axialLength, int wheelDiameter, int gearRatio,
173  int initialLeftTacho, int initialRightTacho,
174  bool enableDPosition, bool enableDeltaPosition)
175 {
176  wheelBase.leftWheelMotor = leftWheelMotor;
177  wheelBase.rightWheelMotor = rightWheelMotor;
178  wheelBase.gearRatio = gearRatio;
179  wheelBase.axialLength = axialLength;
180  wheelBase.wheelDiameter = wheelDiameter;
181  wheelBase.enableDPosition = enableDPosition;
182  wheelBase.enableDeltaPosition = enableDeltaPosition;
183 
184  /* Cumulative integral */
185  VectorInit(wheelBase.cumulativePosition, 0, 0, 0);
186  wheelBase.cumulativeTime = 0;
187  wheelBase.cumulativeSamplesCount = 0;
188 
189  /* Delta integral */
190  VectorInit(wheelBase.deltaPosition, 0, 0, 0);
191  wheelBase.deltaTime = 0;
192  wheelBase.deltaSamplesCount = 0;
193 
194  /* dx and dt */
195  VectorInit(wheelBase.dPosition, 0, 0, 0);
196  wheelBase.dTime = 0;
197 
198  wheelBase.speed = 0;
199  wheelBase.lastTachoLeft = initialLeftTacho;
200  wheelBase.lastTachoRight = initialRightTacho;
201  wheelBase.lastSampleTime = 0;
202 }
203 
208 inline void Odometer2GetCumulativePosition (Odometer2 odometer, Vector &retPosition)
209 {
210  retPosition = odometer.cumulativePosition;
211 }
212 
218 {
219  return odometer.cumulativeSamplesCount;
220 }
221 
226 inline void Odometer2GetDeltaPosition (Odometer2 odometer, Vector &retPosition)
227 {
228  retPosition = odometer.deltaPosition;
229 }
230 
235 {
236  return odometer.deltaSamplesCount;
237 }
238 
249 inline void Odometer2SetDriftCorrection (Odometer2 odometer, Vector &correction,
250  Vector &retDeltaPosition)
251 {
252  int deltaAngle;
253  long deltaDistance;
254 
255  odometer.cumulativePosition = correction;
256 
257  deltaAngle = odometer.deltaPosition.theta + correction.theta;
258  deltaDistance = VectorGetDistance(odometer.deltaPosition);
259 
260  VectorTranslate(odometer.cumulativePosition, deltaDistance, deltaAngle);
261  retDeltaPosition = odometer.deltaPosition;
262  VectorInit(odometer.deltaPosition, 0, 0, 0);
263 }
264 
265 /* test for odometer */
266 #ifdef ENABLE_TEST
267 
268 int lastX = 0;
269 int lastY = 0;
270 void Plot(Vector &pos, int min, int max)
271 {
272  int x = (pos.x - min) * 63 / (max - min);
273  int y = (pos.y - min) * 63 / (max - min);
274 
275  if (x > 63) x = 63;
276  if (y < 0) y = 0;
277 
278  if (lastX != 0 && lastY != 0)
279  LineOut(lastX, lastY, x, y, false);
280  /*
281  int tangentX;
282  int tangentY;
283  if (pos.theta < 90)
284  {
285  tangentX = 10 * Cos(pos.theta);
286  tangentY = 10 * Sin(pos.theta);
287  }
288  else if (pos.theta < 180)
289  {
290  tangentX = -10 * Cos(pos.theta - 90);
291  tangentY = 10 * Sin(pos.theta - 90);
292  }
293  else if (pos.theta < 270)
294  {
295  tangentX = 10 * Cos(pos.theta);
296  tangentY = 10 * Sin(pos.theta);
297  }
298  else
299  {
300  tangentX = 10 * Cos(pos.theta);
301  tangentY = 10 * Sin(pos.theta);
302  }
303  */
304  int tangentX = x + Cos(pos.theta) / 20;
305  int tangentY = y + Sin(pos.theta) / 20;
306 
307  //LineOut(x, y, tangentX, tangentY, false);
308  lastX = x;
309  lastY = y;
310 }
311 
312 #define AXIAL 83
313 #define WHEEL_DIA 56
314 #define SAMPLING_RATE 100
315 void TestOdometer2 ()
316 {
317  OnFwdSync(OUT_BC, 20, 100);
318  Odometer2 robot;
319  //SetOutput(OUT_BC, UpdateFlags, UF_UPDATE_RESET_BLOCK_COUNT);
320  Odometer2Init(robot, OUT_B, OUT_C, AXIAL, WHEEL_DIA, 100, 0, 0, true, true);
321 
322  ClearScreen();
323  LineOut(0, 32, 63, 32, false);
324  LineOut(32, 0, 32, 63, false);
325 
326  while(true)
327  {
328  Vector pos;
329  Odometer2Step(robot);
330  Odometer2GetDeltaPosition(robot, pos);
331  Plot(pos, -550, 550);
332  if (Abs(pos.theta) > 90) break;
333  Wait(50);
334  }
335  Off(OUT_BC);
336  TextOut(0, LCD_LINE1, "Wheel tested");
337  Wait(20000);
338 }
339 
340 #endif /* ENABLE_TEST */
341 #endif /* _ODOMETER2_H_ */
342 
int axialLength
Definition: Odometer2.nxc:52
A vector that represents a 2D point by x-y coordinates and a direction angle.
Definition: Vector.nxc:43
long Odometer2GetDeltaSamplesCount(Odometer2 odometer)
Gets samples count used in current deta integration.
Definition: Odometer2.nxc:234
void Odometer2GetCumulativePosition(Odometer2 odometer, Vector &retPosition)
Gets current cumulative position; position relative to the initial robot position (usually origin in ...
Definition: Odometer2.nxc:208
void Odometer2Init(Odometer2 &wheelBase, int leftWheelMotor, int rightWheelMotor, int axialLength, int wheelDiameter, int gearRatio, int initialLeftTacho, int initialRightTacho, bool enableDPosition, bool enableDeltaPosition)
Initializes a 2 wheel drive odometer.
Definition: Odometer2.nxc:170
long VectorGetDistance(Vector &a)
Gets the polar distance of the given vector. It's the distance of its x-y coordicate from origin...
Definition: Vector.nxc:76
long Odometer2GetCumulativeSamplesCount(Odometer2 odometer)
Gets the total number of sample counts used in cumulative position integration.
Definition: Odometer2.nxc:217
int rightWheelMotor
Definition: Odometer2.nxc:55
Debugging utility macros.
A simple 3-components vector implementation (2D point + direction)
int wheelDiameter
Definition: Odometer2.nxc:51
long theta
Definition: Vector.nxc:47
void Odometer2GetDeltaPosition(Odometer2 odometer, Vector &retPosition)
Gets delta position integration; current position relative to the position when drift was corrected a...
Definition: Odometer2.nxc:226
int leftWheelMotor
Definition: Odometer2.nxc:54
safecall void Odometer2Step(Odometer2 &wheelBase)
Updates current odometry position.
Definition: Odometer2.nxc:93
void VectorTranslate(Vector &a, long distance, int angle)
Translate the vector point to a new point moved the given distance towards the given direction angle...
Definition: Vector.nxc:163
long y
Definition: Vector.nxc:46
int gearRatio
Definition: Odometer2.nxc:53
void Odometer2SetDriftCorrection(Odometer2 odometer, Vector &correction, Vector &retDeltaPosition)
Corrects odometery drift at the position when last drift was corrected.
Definition: Odometer2.nxc:249
long x
Definition: Vector.nxc:45
A 2D map implementation that holds a set of landmark objects.
void VectorInit(Vector &a, long x, long y, long theta)
Initializes the vector with given x, y and theta components.
Definition: Vector.nxc:122
Odometer class for 2 wheels drive robot base that desribes wheel anatomy and positional integration...
Definition: Odometer2.nxc:48