41 #define VEHICLE_MAX_POWER_DEFAULT 60
42 #define VEHICLE_MIN_POWER_DEFAULT 25
43 #define VEHICLE_ACCEL_DEFAULT 3
44 #define VEHICLE_BRAKE_DEFAULT 10
45 #define VEHICLE_MAX_STEERING_FACTOR_DEFAULT 60
46 #define VEHICLE_STABILITY_FACTOR_DEFAULT 210
47 #define VEHICLE_LOOKAHEAD_DISTANCE_DEFAULT 20
80 vehicle.
maxPower = VEHICLE_MAX_POWER_DEFAULT;
81 vehicle.
minPower = VEHICLE_MIN_POWER_DEFAULT;
95 vehicle.
pause =
false;
113 int powerAtSteer,
int steerAtPower,
int maxSteerFactor)
153 vehicle.
pause =
true;
163 vehicle.
pause =
false;
189 int arryLen = ArrayLen(navPoints);
190 for (
int i = 0; i < arryLen; i++)
217 vehicle.
maxPower, -wheelRotation, 300, 0, 0);
241 long steerPower, allowedSteerFactor;
242 long absSteerFactor = Abs(requestedSteerFactor);
248 if (absSteerFactor > 0)
286 if (absSteerFactor > allowedSteerFactor)
294 if (requestedSteerFactor <= 0)
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);
338 Vector currentPoint, moveVector;
339 Vector intersect1, intersect2;
354 intersect1, intersect2);
360 if (d1 < d2) lookahead = intersect1;
361 else lookahead = intersect2;
369 currentPoint, intersect1);
370 intersect2 = intersect1;
387 Vector currentPoint, moveVector, lookahead;
397 if (vehicle.
pause ==
true)
436 long steerFactor = (approachAngle * PAngle)/100;
460 PRIMA_WHEEL_AXIAL_LENGTH,
461 PRIMA_WHEEL_DIAMETER,
462 PRIMA_WHEEL_GEAR_RATIO,
467 VehicleInit(theVehicle, wheelBase, PRIMA_MAX_NAV_POINTS);
Vehicle class representing a 2 wheels drive vehicle.
void MotorStartSync(int motor1, int motor2, int power)
Starts the given 2 motors in synchronous mode with with the given initial power.
void VehicleUpdatePowerAndSteer(Vehicle &vehicle, int requestedSteerFactor)
Updates both current power and current steering of the vehicle based on the requested steering factor...
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...
A vector that represents a 2D point by x-y coordinates and a direction angle.
void VehicleSetDynamics(Vehicle &vehicle, int minPower, int maxPower, int powerAtSteer, int steerAtPower, int maxSteerFactor)
Sets the dynamics of the vehicle.
void Odometer2GetCumulativePosition(Odometer2 odometer, Vector &retPosition)
Gets current cumulative position; position relative to the initial robot position (usually origin in ...
void MotorSetPowerSync(int motor1, int motor2, int power)
Sets the power of the given motor pair.
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 VectorGetDistanceVec(Vector &a, Vector &b)
Gets the distance between given two vectors.
void VehicleAddNavPoint(Vehicle &vehicle, Vector &navPoint)
Adds a navigation point to the internal queue. The navigation point is appended to the queue so that ...
long VehicleSteerToAngle(Vehicle &vehicle, int approachAngle)
Steers the vehicle by the give approachAngle. Positive angle would turn the vehicle anti-clockwise an...
void VehicleInit(Vehicle &vehicle, Odometer2 &odometer, int maxNavPoints)
Initializes Vehicle with the given odometer and maximum amount of navigation points internal queue sh...
void VehiclePause(Vehicle &vehicle)
Pauses the vehicle. The pause is in effect immediately.
Implements odometery for 2 wheels drive robot base using motor tachometers.
void VectorReduce(Vector &a, long S)
Reduces x and y vector components by the given factor. i.e. a = a / S.
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.
void VehicleSetAcceleration(Vehicle &vehicle, int accelerationPerStep, int brakingPerStep)
Sets the acceleration and braking rate of the vehicle.
safecall void Odometer2Step(Odometer2 &wheelBase)
Updates current odometry position.
void MotorStopSync(int motor1, int motor2)
Stops the given two motor pair.
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...
int VehicleStep(Vehicle &vehicle)
Vehicle execution step funtion.
Provides simple arithmetic for absolute and relative angles.
int AngleAbsToRel(int absAngle)
Converts absolute angle (0 .. 360) to relative angle (-180 .. 180)
int AngleAbsSub(int absAngleA, int angle)
Subtracts angle from an absolute angle (0 .. 360) absAngleA. Returns an absolute angle properly trunc...
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. ...
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...
bool VehicleGetLookahead(Vehicle &vehicle, Vector &lookahead)
Determines the current lookahead point of the vehicle.
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...
Motor control implementation.
void VehicleContinue(Vehicle &vehicle)
Continues the vehicle. The continue is in effect immediately and will start accelerating from next st...
Circle geometry operations.
long VectorGetAngleVec(Vector &a, Vector &b)
Gets the angle of the line formed by the given two vectors.
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...