A simple 2 wheels drive vehicle implementation that can travel a set of navigation points. More...
#include "Debug.nxc"#include "Angle.nxc"#include "Vector.nxc"#include "Circle.nxc"#include "Odometer2.nxc"#include "Motor.nxc"#include "RobotPrima.nxc"
Go to the source code of this file.
Data Structures | |
| struct | Vehicle |
| Vehicle class representing a 2 wheels drive vehicle. More... | |
Functions | |
| void | VehicleInit (Vehicle &vehicle, Odometer2 &odometer, int maxNavPoints) |
| Initializes Vehicle with the given odometer and maximum amount of navigation points internal queue should hold. | |
| void | VehicleSetDynamics (Vehicle &vehicle, int minPower, int maxPower, int powerAtSteer, int steerAtPower, int maxSteerFactor) |
| Sets the dynamics of the vehicle. More... | |
| void | VehicleSetAcceleration (Vehicle &vehicle, int accelerationPerStep, int brakingPerStep) |
| Sets the acceleration and braking rate of the vehicle. More... | |
| void | VehicleSetLookaheadDistance (Vehicle &vehicle, int lookaheadDistance) |
| Sets the look ahead distance of the vehicle. More... | |
| void | VehiclePause (Vehicle &vehicle) |
| Pauses the vehicle. The pause is in effect immediately. | |
| void | VehicleContinue (Vehicle &vehicle) |
| Continues the vehicle. The continue is in effect immediately and will start accelerating from next step on wards. | |
| void | VehicleAddNavPoint (Vehicle &vehicle, Vector &navPoint) |
| Adds a navigation point to the internal queue. The navigation point is appended to the queue so that it is executed after existing navigation points. Navigation points vectors in cm. | |
| 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 they are executed after existing navigation points. Navigation points vectors in cm. | |
| long | VehicleSteerToAngle (Vehicle &vehicle, int approachAngle) |
| Steers the vehicle by the give approachAngle. Positive angle would turn the vehicle anti-clockwise and negative angle would turn it clockwise. More... | |
| void | VehicleUpdatePowerAndSteer (Vehicle &vehicle, int requestedSteerFactor) |
| Updates both current power and current steering of the vehicle based on the requested steering factor. More... | |
| 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. | |
| bool | VehicleGetLookahead (Vehicle &vehicle, Vector &lookahead) |
| Determines the current lookahead point of the vehicle. More... | |
| int | VehicleStep (Vehicle &vehicle) |
| Vehicle execution step funtion. More... | |
Detailed Description
A simple 2 wheels drive vehicle implementation that can travel a set of navigation points.
It contains a pair of differential motors (independently controlled), uses odometry to position itself in 2D plane and a set of defined navigation points. The navigation points can be dynamically changed during run time. All vectors operated in Vehicle are in centimeters (cm).
Definition in file Vehicle.nxc.
Function Documentation
Determines the current lookahead point of the vehicle.
Look ahead point is a point 'look ahead' distance away from the vehicle that intersects with the next path that vehicle is supposed to follow. 'Next path' is the path denoted by the line from last visited navigation point to the currently targeted navigation point. If there is no intersection (that is, vehicle is off the path too much) it uses the closest point (within the circle) to the path.
Definition at line 336 of file Vehicle.nxc.
References CircleIntersectLine(), Vehicle::lastMoveVector, LineProjectPoint(), Vehicle::lookaheadDistance, Vehicle::navPoint, Vehicle::navPointLast, Vehicle::navPointsQueue, Vehicle::odometer, Odometer2GetCumulativePosition(), VectorGetDistanceVec(), and VectorReduce().
Referenced by VehicleStep().
| void VehicleSetAcceleration | ( | Vehicle & | vehicle, |
| int | accelerationPerStep, | ||
| int | brakingPerStep | ||
| ) |
Sets the acceleration and braking rate of the vehicle.
When vehicle needs acceleration or braking, accelerationPerStep or brakingPerStep will be used to increase or reduce the power per step.
Definition at line 127 of file Vehicle.nxc.
References Vehicle::accelPower, and Vehicle::brakePower.
| void VehicleSetDynamics | ( | Vehicle & | vehicle, |
| int | minPower, | ||
| int | maxPower, | ||
| int | powerAtSteer, | ||
| int | steerAtPower, | ||
| int | maxSteerFactor | ||
| ) |
Sets the dynamics of the vehicle.
minPower is the minimum power that the vehicle should maintain as long as it has navigation paths to follow. maxPower is the maximum power the vehicle is allowed to take. powerAtSteer and steerAtPower are one power and steering factor (respectively) combination found in inverse relationship of power and steering factor funtion. This is used to determine stability factor of the vehicle. maxSteerFactor is the maximum steering factor allowed to take by the vehicle.
Definition at line 112 of file Vehicle.nxc.
References Vehicle::maxPower, Vehicle::maxSteerFactor, Vehicle::minPower, and Vehicle::stabilityFactor.
| void VehicleSetLookaheadDistance | ( | Vehicle & | vehicle, |
| int | lookaheadDistance | ||
| ) |
Sets the look ahead distance of the vehicle.
The vehicle implements 'carrot following' algorithm for path following. Look ahead distance is the distance of the carrot from vehicle to determine look ahead point (carrot).
Definition at line 141 of file Vehicle.nxc.
References Vehicle::lookaheadDistance.
| long VehicleSteerToAngle | ( | Vehicle & | vehicle, |
| int | approachAngle | ||
| ) |
Steers the vehicle by the give approachAngle. Positive angle would turn the vehicle anti-clockwise and negative angle would turn it clockwise.
This is a direct in-place turn and motors will be stopped after the turn. It uses PID control to make the turn.
Definition at line 212 of file Vehicle.nxc.
References Odometer2::axialLength, Odometer2::gearRatio, Vehicle::maxPower, Vehicle::motorLeft, Vehicle::motorRight, MotorRotateAngleDiff(), Vehicle::odometer, Odometer2Step(), and Odometer2::wheelDiameter.
| int VehicleStep | ( | Vehicle & | vehicle | ) |
Vehicle execution step funtion.
Call this at regular interval (not necessarily with the same interval period). 30 to 150 ms is a good choice. This will execute vehicle commands required to move it through the series of navigation points. Each step is a minuscle operation to correct the drive operations.
Definition at line 384 of file Vehicle.nxc.
References AngleAbsSub(), AngleAbsToRel(), Vehicle::lastMoveVector, Vehicle::lookaheadDistance, Vehicle::maxSteerFactor, Vehicle::motorLeft, Vehicle::motorRight, MotorStopSync(), Vehicle::navPoint, Vehicle::navPointLast, Vehicle::navPointsQueue, Vehicle::odometer, Odometer2GetCumulativePosition(), Odometer2Step(), Vehicle::pause, Vector::theta, VectorGetAngleVec(), VectorGetDistanceVec(), VectorReduce(), VehicleGetLookahead(), and VehicleUpdatePowerAndSteer().
| void VehicleUpdatePowerAndSteer | ( | Vehicle & | vehicle, |
| int | requestedSteerFactor | ||
| ) |
Updates both current power and current steering of the vehicle based on the requested steering factor.
Vehicle may not get the requested steering if the current power is too high for it. It will instead try to slow down to the power level where this steering is possible. On the other hand if requested steering is low for the current power, it will try to accelarate the vehicle. The stable and safe steering factor at a given power is computate as an inverse function – that is, higher the power, lower the allowable steering factor. The constant in this function is the 'stability factor'. The vehicle always try to keep the set minimum power to motors as long as it is traveling, and maximum power as long as it is going straight. Between the two extremes, the vehicle will accelerate or decelarate depending on the steering needs.
Definition at line 239 of file Vehicle.nxc.
References Vehicle::accelPower, Vehicle::brakePower, Vehicle::currentPower, Vehicle::currentSteerFactor, Vehicle::maxPower, Vehicle::maxSteerFactor, Vehicle::minPower, Vehicle::motorLeft, Vehicle::motorRight, MotorSetTurnRatioPower(), and Vehicle::stabilityFactor.
Referenced by VehicleStep().