libnxter  0.1
Vehicle.nxc
Go to the documentation of this file.
1 /* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
2 /*
3  Vehicle.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 */
31 #ifndef _VEHICLE_H_
32 #define _VEHICLE_H_
33 
34 #include "Debug.nxc"
35 #include "Angle.nxc"
36 #include "Vector.nxc"
37 #include "Circle.nxc"
38 #include "Odometer2.nxc"
39 #include "Motor.nxc"
40 
41 #define VEHICLE_MAX_POWER_DEFAULT 60
42 #define VEHICLE_MIN_POWER_DEFAULT 25
43 #define VEHICLE_ACCEL_DEFAULT 3 /* Acceleration */
44 #define VEHICLE_BRAKE_DEFAULT 10 /* Deceleration */
45 #define VEHICLE_MAX_STEERING_FACTOR_DEFAULT 60
46 #define VEHICLE_STABILITY_FACTOR_DEFAULT 210 /* Max 3 steer factor at 90 power */
47 #define VEHICLE_LOOKAHEAD_DISTANCE_DEFAULT 20 /* Look ahead distance 20 cm */
48 
52 struct Vehicle
53 {
54  int maxPower;
55  int minPower;
56  int accelPower;
57  int brakePower;
62  int motorLeft;
63  int motorRight;
66  int navPoint;
71  bool pause;
72 };
73 
78 void VehicleInit(Vehicle &vehicle, Odometer2 &odometer, int maxNavPoints)
79 {
80  vehicle.maxPower = VEHICLE_MAX_POWER_DEFAULT;
81  vehicle.minPower = VEHICLE_MIN_POWER_DEFAULT;
82  vehicle.accelPower = VEHICLE_ACCEL_DEFAULT;
83  vehicle.brakePower = VEHICLE_BRAKE_DEFAULT;
84  vehicle.currentPower = 0;
85  vehicle.maxSteerFactor = VEHICLE_MAX_STEERING_FACTOR_DEFAULT;
86  vehicle.currentSteerFactor = 0;
87  vehicle.stabilityFactor = VEHICLE_STABILITY_FACTOR_DEFAULT;
88  vehicle.motorLeft = odometer.leftWheelMotor;
89  vehicle.motorRight = odometer.rightWheelMotor;
90 
91  ArrayInit(vehicle.navPointsQueue, 0, maxNavPoints);
92  vehicle.navPoint = 0;
93  vehicle.navPointLast = 0;
94  vehicle.maxNavPoints = maxNavPoints;
95  vehicle.pause = false;
96  vehicle.odometer = odometer;
97  VectorInit(vehicle.lastMoveVector, 0, 0, 0);
98  vehicle.lookaheadDistance = VEHICLE_LOOKAHEAD_DISTANCE_DEFAULT;
99 }
100 
112 void VehicleSetDynamics(Vehicle &vehicle, int minPower, int maxPower,
113  int powerAtSteer, int steerAtPower, int maxSteerFactor)
114 {
115  vehicle.minPower = minPower;
116  vehicle.maxPower = maxPower;
117  vehicle.stabilityFactor = (powerAtSteer - minPower) * steerAtPower;
118  vehicle.maxSteerFactor = maxSteerFactor;
119 }
120 
127 void VehicleSetAcceleration(Vehicle &vehicle, int accelerationPerStep,
128  int brakingPerStep)
129 {
130  vehicle.accelPower = accelerationPerStep;
131  vehicle.brakePower = brakingPerStep;
132 }
133 
141 void VehicleSetLookaheadDistance(Vehicle &vehicle, int lookaheadDistance)
142 {
143  vehicle.lookaheadDistance = lookaheadDistance;
144 }
145 
149 void VehiclePause(Vehicle &vehicle)
150 {
151  MotorSetPowerSync(vehicle.motorLeft, vehicle.motorRight, 0);
152  vehicle.currentPower = 0;
153  vehicle.pause = true;
154 }
155 
160 void VehicleContinue(Vehicle &vehicle)
161 {
162  MotorStartSync(vehicle.motorLeft, vehicle.motorRight, 0);
163  vehicle.pause = false;
164 }
165 
171 void VehicleAddNavPoint(Vehicle &vehicle, Vector &navPoint)
172 {
173  if (vehicle.navPointLast >= vehicle.maxNavPoints)
174  vehicle.navPointLast = 0;
175  vehicle.navPointsQueue[vehicle.navPointLast] = navPoint;
176  vehicle.navPointLast++;
177 
178  /* Start drive */
179  MotorStartSync(vehicle.motorLeft, vehicle.motorRight, 0);
180 }
181 
187 void VehicleAddNavPoints(Vehicle &vehicle, Vector &navPoints[])
188 {
189  int arryLen = ArrayLen(navPoints);
190  for (int i = 0; i < arryLen; i++)
191  {
192  if (vehicle.navPointLast >= vehicle.maxNavPoints)
193  vehicle.navPointLast = 0;
194 
195  Vector v = navPoints[i];
196  vehicle.navPointsQueue[vehicle.navPointLast] = v;
197  vehicle.navPointLast++;
198  }
199  /* Start drive */
200  if (arryLen > 0)
201  MotorStartSync(vehicle.motorLeft, vehicle.motorRight, 0);
202 }
203 
212 long VehicleSteerToAngle(Vehicle &vehicle, int approachAngle)
213 {
214  long wheelRotation = (approachAngle * vehicle.odometer.axialLength * 100) /
215  (vehicle.odometer.wheelDiameter * vehicle.odometer.gearRatio);
216  MotorRotateAngleDiff(vehicle.motorLeft, vehicle.motorRight,
217  vehicle.maxPower, -wheelRotation, 300, 0, 0);
218 
219  /* Update odometer after the steer */
220  Odometer2Step(vehicle.odometer);
221 }
222 
239 void VehicleUpdatePowerAndSteer(Vehicle &vehicle, int requestedSteerFactor)
240 {
241  long steerPower, allowedSteerFactor;
242  long absSteerFactor = Abs(requestedSteerFactor);
243 
244  /* Avoid divide by 0. If steer factor is 0 (i.e. go straight) assign
245  * maximum power for the allowable steering power. Otherwise, calculate
246  * it from the stability factor.
247  */
248  if (absSteerFactor > 0)
249  steerPower = vehicle.minPower + vehicle.stabilityFactor/absSteerFactor;
250  else
251  steerPower = vehicle.maxPower;
252 
253  /* Update power based on requested steerFactor. If allowable power is
254  * is greater than current power, the steering can easily happen and
255  * also the motors can increase their powers -- accelerate slowly.
256  */
257  if (steerPower > vehicle.currentPower)
258  {
259  vehicle.currentPower += vehicle.accelPower;
260  if (vehicle.currentPower > vehicle.maxPower)
261  vehicle.currentPower = vehicle.maxPower;
262 
263  }
264  /* Otherwise, if allowable power is less than current power, deccelerate
265  * to reach the allowed power for making necessary steering.
266  */
267  else if (steerPower < vehicle.currentPower)
268  {
269  vehicle.currentPower -= vehicle.brakePower;
270  if (vehicle.currentPower < 0)
271  vehicle.currentPower = 0;
272  }
273 
274  /* Determine the limit of steering that can be safely done on current power */
275  long powerDiff = (vehicle.currentPower - vehicle.minPower);
276  if (powerDiff <= 0)
277  allowedSteerFactor = vehicle.maxSteerFactor;
278  else
279  allowedSteerFactor = vehicle.stabilityFactor/powerDiff;
280  if (allowedSteerFactor > vehicle.maxSteerFactor)
281  allowedSteerFactor = vehicle.maxSteerFactor;
282 
283  /* Pick whichever is minimum, the request steering or currently safe
284  * steering calculated above based on current power.
285  */
286  if (absSteerFactor > allowedSteerFactor)
287  {
288  /* Requested steering factor is above the limit possible with current
289  * power, so use the limit instead while the vehicle decelarates.
290  */
291  vehicle.currentSteerFactor = allowedSteerFactor;
292 
293  /* Correct current steering factor sign */
294  if (requestedSteerFactor <= 0)
295  vehicle.currentSteerFactor = -vehicle.currentSteerFactor;
296  }
297  else
298  {
299  /* Requested steering factor is within the limit of current power
300  * so use it directly.
301  */
302  vehicle.currentSteerFactor = requestedSteerFactor;
303  }
304 
305  /* Apply the powers to wheels */
307  vehicle.currentSteerFactor,
308  vehicle.currentPower);
309 
310 }
311 
317 {
318  long dsqr = VectorGetDistanceVec(a, b);
319  dsqr *= dsqr;
320  long u = ((p.x - a.x) * (b.x - a.x) + (p.y - a.y) * (b.y - a.y))/dsqr;
321  ret.x = a.x + u * (b.x - a.x);
322  ret.y = a.y + u * (b.y - a.y);
323  ret.theta = 0;
324 }
325 
336 bool VehicleGetLookahead(Vehicle &vehicle, Vector &lookahead)
337 {
338  Vector currentPoint, moveVector;
339  Vector intersect1, intersect2;
340  bool result;
341 
342  if (vehicle.navPoint == vehicle.navPointLast)
343  {
344  /* Vehicle has no currently targeted navigation point */
345  return false;
346  }
347 
348  Odometer2GetCumulativePosition(vehicle.odometer, currentPoint);
349  VectorReduce(currentPoint, 10); /* Reduce to cm */
350 
351  moveVector = vehicle.navPointsQueue[vehicle.navPoint];
352  result = CircleIntersectLine(currentPoint, vehicle.lookaheadDistance,
353  vehicle.lastMoveVector, moveVector,
354  intersect1, intersect2);
355  if (result == true)
356  {
357  /* Pick the intersection point closer to move vector */
358  long d1 = VectorGetDistanceVec(moveVector, intersect1);
359  long d2 = VectorGetDistanceVec(moveVector, intersect2);
360  if (d1 < d2) lookahead = intersect1;
361  else lookahead = intersect2;
362  }
363  else
364  {
365  /* If we don't get a lookahead circle intersection, find a projected
366  * target on the path to follow
367  */
368  LineProjectPoint(vehicle.lastMoveVector, moveVector,
369  currentPoint, intersect1);
370  intersect2 = intersect1;
371  }
372  return true;
373 }
374 
384 int VehicleStep(Vehicle &vehicle)
385 {
386  long PAngle = 50;
387  Vector currentPoint, moveVector, lookahead;
388 
389  if(vehicle.navPoint == vehicle.navPointLast)
390  {
391  MotorStopSync(vehicle.motorLeft, vehicle.motorRight);
392  /* Nowhere to move */
393  return false;
394  }
395 
396  /* Check pause */
397  if (vehicle.pause == true)
398  {
399  /* Step must be continued */
400  return true;
401  }
402 
403  Odometer2Step(vehicle.odometer);
404  Odometer2GetCumulativePosition(vehicle.odometer, currentPoint);
405  VectorReduce(currentPoint, 10); /* Convert to cm */
406 
407  moveVector = vehicle.navPointsQueue[vehicle.navPoint];
408 
409  /* Determine distance to destination */
410  long distanceToTravel = VectorGetDistanceVec(currentPoint, moveVector);
411 
412  /* If current move vector is reached, move on to next vector */
413  if (distanceToTravel < vehicle.lookaheadDistance)
414  {
415  vehicle.navPoint++;
416  vehicle.lastMoveVector = moveVector;
417 
418  /* Step must be continued */
419  return true;
420  }
421 
422  /* Look ahead and find the direction of heading */
423  VehicleGetLookahead(vehicle, lookahead);
424 
425  /* Determine move angle (0 .. 360), which is the (absolute) target
426  * angle to which vehicle is to be steered.
427  */
428  long moveAngle = VectorGetAngleVec(currentPoint, lookahead);
429 
430  /* Determine approach angle (0 .. 360) from vehicle's orientation */
431  long approachAngle = AngleAbsSub(moveAngle, currentPoint.theta);
432 
433  /* Determine approachAngle relative -180 .. 180 */
434  approachAngle = AngleAbsToRel(approachAngle);
435 
436  long steerFactor = (approachAngle * PAngle)/100;
437  if (steerFactor > vehicle.maxSteerFactor)
438  steerFactor = vehicle.maxSteerFactor;
439  else if (steerFactor < -vehicle.maxSteerFactor)
440  steerFactor = -vehicle.maxSteerFactor;
441 
442  VehicleUpdatePowerAndSteer(vehicle, steerFactor);
443 
444  /* Step must be continued */
445  return true;
446 }
447 
448 /* Tests */
449 #ifdef ENABLE_TEST
450 #include "RobotPrima.nxc"
451 
452 void TestVehicle()
453 {
454  Vehicle theVehicle;
455  Odometer2 wheelBase;
456  Vector point;
457 
458  /* Initialize vehicle */
459  Odometer2Init(wheelBase, OUT_B, OUT_C,
460  PRIMA_WHEEL_AXIAL_LENGTH,
461  PRIMA_WHEEL_DIAMETER,
462  PRIMA_WHEEL_GEAR_RATIO,
463  0, /* Initial left tacho count */
464  0, /* Initial right tacho count */
465  true, /* enable dx calculation */
466  true); /* enable delta calculation */
467  VehicleInit(theVehicle, wheelBase, PRIMA_MAX_NAV_POINTS);
468 
469  /* Set navigation path */
470 
471  VectorInit(point, 100, 0, 0);
472  VehicleAddNavPoint(theVehicle, point);
473  VectorInit(point, 100, 50, 0);
474  VehicleAddNavPoint(theVehicle, point);
475  VectorInit(point, 0, 50, 0);
476  VehicleAddNavPoint(theVehicle, point);
477  VectorInit(point, 0, 0, 0);
478  VehicleAddNavPoint(theVehicle, point);
479  VectorInit(point, 30, 50, 0);
480  VehicleAddNavPoint(theVehicle, point);
481  VectorInit(point, 70, -50, 0);
482  VehicleAddNavPoint(theVehicle, point);
483  VectorInit(point, 100, 0, 0);
484  VehicleAddNavPoint(theVehicle, point);
485  VectorInit(point, 50, -20, 0);
486  VehicleAddNavPoint(theVehicle, point);
487  VectorInit(point, 0, 0, 0);
488  VehicleAddNavPoint(theVehicle, point);
489 
490  while(VehicleStep(theVehicle)) Wait(50);
491 }
492 
493 #endif /* ENABLE_TEST */
494 #endif /* _VEHICLE_H_ */
495 
Vehicle class representing a 2 wheels drive vehicle.
Definition: Vehicle.nxc:52
void MotorStartSync(int motor1, int motor2, int power)
Starts the given 2 motors in synchronous mode with with the given initial power.
Definition: Motor.nxc:71
void VehicleUpdatePowerAndSteer(Vehicle &vehicle, int requestedSteerFactor)
Updates both current power and current steering of the vehicle based on the requested steering factor...
Definition: Vehicle.nxc:239
int maxPower
Definition: Vehicle.nxc:54
int motorRight
Definition: Vehicle.nxc:63
bool CircleIntersectLine(Vector &circle, long radius, Vector &pointA, Vector &pointB, Vector &retIntersect1, Vector &retIntersect2)
Determins the 2 points of intersection of a circle and a line given by two vector points...
Definition: Circle.nxc:99
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
void VehicleSetDynamics(Vehicle &vehicle, int minPower, int maxPower, int powerAtSteer, int steerAtPower, int maxSteerFactor)
Sets the dynamics of the vehicle.
Definition: Vehicle.nxc:112
int navPoint
Definition: Vehicle.nxc:66
Vector lastMoveVector
Definition: Vehicle.nxc:70
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 MotorSetPowerSync(int motor1, int motor2, int power)
Sets the power of the given motor pair.
Definition: Motor.nxc:102
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
int minPower
Definition: Vehicle.nxc:55
long VectorGetDistanceVec(Vector &a, Vector &b)
Gets the distance between given two vectors.
Definition: Vector.nxc:100
bool pause
Definition: Vehicle.nxc:71
void VehicleAddNavPoint(Vehicle &vehicle, Vector &navPoint)
Adds a navigation point to the internal queue. The navigation point is appended to the queue so that ...
Definition: Vehicle.nxc:171
int brakePower
Definition: Vehicle.nxc:57
long VehicleSteerToAngle(Vehicle &vehicle, int approachAngle)
Steers the vehicle by the give approachAngle. Positive angle would turn the vehicle anti-clockwise an...
Definition: Vehicle.nxc:212
int rightWheelMotor
Definition: Odometer2.nxc:55
void VehicleInit(Vehicle &vehicle, Odometer2 &odometer, int maxNavPoints)
Initializes Vehicle with the given odometer and maximum amount of navigation points internal queue sh...
Definition: Vehicle.nxc:78
void VehiclePause(Vehicle &vehicle)
Pauses the vehicle. The pause is in effect immediately.
Definition: Vehicle.nxc:149
Implements odometery for 2 wheels drive robot base using motor tachometers.
int stabilityFactor
Definition: Vehicle.nxc:61
void VectorReduce(Vector &a, long S)
Reduces x and y vector components by the given factor. i.e. a = a / S.
Definition: Vector.nxc:201
Debugging utility macros.
A simple 3-components vector implementation (2D point + direction)
void VehicleSetLookaheadDistance(Vehicle &vehicle, int lookaheadDistance)
Sets the look ahead distance of the vehicle.
Definition: Vehicle.nxc:141
int wheelDiameter
Definition: Odometer2.nxc:51
int maxNavPoints
Definition: Vehicle.nxc:68
Odometer2 odometer
Definition: Vehicle.nxc:64
long theta
Definition: Vector.nxc:47
int maxSteerFactor
Definition: Vehicle.nxc:59
int currentPower
Definition: Vehicle.nxc:58
int leftWheelMotor
Definition: Odometer2.nxc:54
void VehicleSetAcceleration(Vehicle &vehicle, int accelerationPerStep, int brakingPerStep)
Sets the acceleration and braking rate of the vehicle.
Definition: Vehicle.nxc:127
safecall void Odometer2Step(Odometer2 &wheelBase)
Updates current odometry position.
Definition: Odometer2.nxc:93
void MotorStopSync(int motor1, int motor2)
Stops the given two motor pair.
Definition: Motor.nxc:88
int MotorRotateAngleDiff(int motor1, int motor2, int power, long angle, long PValue, long IValue, long DValue)
Rotates the given pair of motors differentially (that is they rotate at the same pace in opposite dir...
Definition: Motor.nxc:211
int VehicleStep(Vehicle &vehicle)
Vehicle execution step funtion.
Definition: Vehicle.nxc:384
Provides simple arithmetic for absolute and relative angles.
long y
Definition: Vector.nxc:46
int motorLeft
Definition: Vehicle.nxc:62
int navPointLast
Definition: Vehicle.nxc:67
int gearRatio
Definition: Odometer2.nxc:53
long x
Definition: Vector.nxc:45
int AngleAbsToRel(int absAngle)
Converts absolute angle (0 .. 360) to relative angle (-180 .. 180)
Definition: Angle.nxc:44
int AngleAbsSub(int absAngleA, int angle)
Subtracts angle from an absolute angle (0 .. 360) absAngleA. Returns an absolute angle properly trunc...
Definition: Angle.nxc:76
int currentSteerFactor
Definition: Vehicle.nxc:60
An experimental robotic platform for developing this library.
void LineProjectPoint(Vector a, Vector b, Vector p, Vector &ret)
Determines the perpendicular drop of point p on the line given by points a and b. ...
Definition: Vehicle.nxc:316
int lookaheadDistance
Definition: Vehicle.nxc:69
void MotorSetTurnRatioPower(int motor1, int motor2, int turnRatio, int power)
Sets the turn ratio and power at the same time of the given motor pair running in synchronous mode...
Definition: Motor.nxc:124
bool VehicleGetLookahead(Vehicle &vehicle, Vector &lookahead)
Determines the current lookahead point of the vehicle.
Definition: Vehicle.nxc:336
void VehicleAddNavPoints(Vehicle &vehicle, Vector &navPoints[])
Adds a set of navigation points to the internal queue. The points are appended to the queue so that t...
Definition: Vehicle.nxc:187
int accelPower
Definition: Vehicle.nxc:56
Motor control implementation.
Vector navPointsQueue[]
Definition: Vehicle.nxc:65
void VehicleContinue(Vehicle &vehicle)
Continues the vehicle. The continue is in effect immediately and will start accelerating from next st...
Definition: Vehicle.nxc:160
Circle geometry operations.
long VectorGetAngleVec(Vector &a, Vector &b)
Gets the angle of the line formed by the given two vectors.
Definition: Vector.nxc:110
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