42 #define INFINITY 2147483647
60 long cumulativeSamplesCount;
63 bool enableDeltaPosition;
66 long deltaSamplesCount;
95 long tachoLeft = GetOutput(wheelBase.
leftWheelMotor, RotationCount);
97 long dLeftTacho = (tachoLeft - wheelBase.lastTachoLeft);
98 long dRightTacho = (tachoRight - wheelBase.lastTachoRight);
99 long tick = CurrentTick();
105 long v = (((dLeftTacho + dRightTacho) *
107 long w = ((dRightTacho - dLeftTacho) * wheelBase.
wheelDiameter *
111 if (wheelBase.enableDPosition)
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;
120 if (wheelBase.enableDeltaPosition)
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++;
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++;
152 wheelBase.lastTachoLeft = tachoLeft;
153 wheelBase.lastTachoRight = tachoRight;
154 wheelBase.lastSampleTime = tick;
155 wheelBase.speed = (v * 1000) / wheelBase.dTime;
171 int leftWheelMotor,
int rightWheelMotor,
172 int axialLength,
int wheelDiameter,
int gearRatio,
173 int initialLeftTacho,
int initialRightTacho,
174 bool enableDPosition,
bool enableDeltaPosition)
181 wheelBase.enableDPosition = enableDPosition;
182 wheelBase.enableDeltaPosition = enableDeltaPosition;
185 VectorInit(wheelBase.cumulativePosition, 0, 0, 0);
186 wheelBase.cumulativeTime = 0;
187 wheelBase.cumulativeSamplesCount = 0;
191 wheelBase.deltaTime = 0;
192 wheelBase.deltaSamplesCount = 0;
199 wheelBase.lastTachoLeft = initialLeftTacho;
200 wheelBase.lastTachoRight = initialRightTacho;
201 wheelBase.lastSampleTime = 0;
210 retPosition = odometer.cumulativePosition;
219 return odometer.cumulativeSamplesCount;
228 retPosition = odometer.deltaPosition;
236 return odometer.deltaSamplesCount;
255 odometer.cumulativePosition = correction;
257 deltaAngle = odometer.deltaPosition.
theta + correction.
theta;
260 VectorTranslate(odometer.cumulativePosition, deltaDistance, deltaAngle);
261 retDeltaPosition = odometer.deltaPosition;
270 void Plot(
Vector &pos,
int min,
int max)
272 int x = (pos.
x - min) * 63 / (max - min);
273 int y = (pos.
y - min) * 63 / (max - min);
278 if (lastX != 0 && lastY != 0)
279 LineOut(lastX, lastY, x, y,
false);
304 int tangentX = x + Cos(pos.
theta) / 20;
305 int tangentY = y + Sin(pos.
theta) / 20;
314 #define SAMPLING_RATE 100
315 void TestOdometer2 ()
317 OnFwdSync(OUT_BC, 20, 100);
320 Odometer2Init(robot, OUT_B, OUT_C, AXIAL, WHEEL_DIA, 100, 0, 0,
true,
true);
323 LineOut(0, 32, 63, 32,
false);
324 LineOut(32, 0, 32, 63,
false);
331 Plot(pos, -550, 550);
332 if (Abs(pos.
theta) > 90)
break;
336 TextOut(0, LCD_LINE1,
"Wheel tested");
A vector that represents a 2D point by x-y coordinates and a direction angle.
long Odometer2GetDeltaSamplesCount(Odometer2 odometer)
Gets samples count used in current deta integration.
void Odometer2GetCumulativePosition(Odometer2 odometer, Vector &retPosition)
Gets current cumulative position; position relative to the initial robot position (usually origin in ...
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.
long VectorGetDistance(Vector &a)
Gets the polar distance of the given vector. It's the distance of its x-y coordicate from origin...
long Odometer2GetCumulativeSamplesCount(Odometer2 odometer)
Gets the total number of sample counts used in cumulative position integration.
Debugging utility macros.
A simple 3-components vector implementation (2D point + direction)
void Odometer2GetDeltaPosition(Odometer2 odometer, Vector &retPosition)
Gets delta position integration; current position relative to the position when drift was corrected a...
safecall void Odometer2Step(Odometer2 &wheelBase)
Updates current odometry position.
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...
void Odometer2SetDriftCorrection(Odometer2 odometer, Vector &correction, Vector &retDeltaPosition)
Corrects odometery drift at the position when last drift was corrected.
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.
Odometer class for 2 wheels drive robot base that desribes wheel anatomy and positional integration...