MoxCodes22
commented
7 months ago Result
Control.bd is created that will contain the control functions of the curve following functionality.
Check if next focuspoint needs to be approached:
Calculation of $\alpha$:
tmp holds $(1;0)$. Furthermore, the Route.bd has been rewritten to return a point now, rather than two coordinates x and y:
The steering control algorithm has been sourced out to a SteeringCtrl block diagram that can be reused for any test driver to be created in the future:
This block diagram ensures that #3 and #4 are adhered to, setting dstr to 0.01 and ensuring that absolute max. steering is not greater than 0.5:
After refactoring:
The velocity controller is added, too:
To come to a stop at the last focuspoint, we set the target velocity to 0 once the distance is equal or smaller than the breaking distance.
Frankybeen29
Feasibility
The picture provided in the task description explains how to calculate the angle between the vector from the current car position to the next focus point and the vector pointing in the direction of the car's bearing.
In the provided example, a rotation operation along with a signum block is used to ensure that the angle is in intervall [-pi;pi].
But is there an easier way?
Yes, there is. To calculate the angle between two vectors, one can use the atan2 function. The great advantage here, is that it's co-domain is in [-pi;pi], so there is no need for any rotation operation or signum blocks. Also since we can calculate the angle between two arbitrary vectors with the atan2 function, we can just calculate the angle between the x-axis and the vector to the next focus point and subtract the bearing angle from it to obtain $\alpha$:
$\alpha = atan2(\vec{v} \times \vec{w}, \vec{v} \cdot \vec{w}) - \beta$
With and $\vec{v} = (1;0)$ and $\vec{w}$ being the vector to the next focus point.
(see http://johnblackburne.blogspot.com/2012/05/angle-between-two-3d-vectors.html and https://mathworld.wolfram.com/InverseTangent.html)
The cross product and the dot product have both been implemented in the vectorOps.esdl, the function for angle calculation needs to be adapted accordingly. Therefore these functions can be used to restructure the block diagram.
Can we maintain the 60 kmph over most of the track? When accelerating at max. power, we can get to 60 kmph in under 4 seconds.
The driven distance is $d = 3.9s * 60kmph / 2 \approx 32.5m$. The route is probably round about 600m long (estimation).
For max. breaking, we get a breaking distance of approximately the same distance:
$d = 4s * 60kmph / 2 \approx 33m$
Therefore we can easily drive the route with 60kmph most of the time.
In terms of steering, the maximum change in bearing is achieved when steering is equal to $k \cdot \pi/2$ and minimum change when steering equals $k \cdot \pi$ with $k$ being an integer.
One can therefore assume the change in bearing to follow a sine curve and this can be validated further by looking into the code in MyTurn.esdl:
bearing = bearing + (ds/D)*Lib.sin(beta);
But since we act between a steering of -0.5 to 0.5, we don't need to worry about the periodicity here.
Limitations
One problem we have is that $\alpha$ can, as of now, be between [-2pi;2pi] since $\beta$ is in [-pi;pi], so is the result from $tan2$. Therefore, when calculating the difference between two angles, we always desire the result to be in [-pi;pi]. We can achieve this by applying followiing formula:
$diff_{actual} = -2 \cdot sign(diff) \cdot \pi + diff \ \ \ \ \ if \ \ |diff| > \pi \ \ \ \ \ else \ \ = diff$ with $diff = angle_1 - angle_2$
Example:
The vector in brown is the vector to the next focuspoint, the vector in blue is the bearing vector of the car. As explained above we would calculate $\alpha = \gamma - \beta = 150° + 170° = 320°$
Applying the formula we get: $diff_{actual} = -2 \cdot sign(diff) \cdot \pi + diff = -2 \cdot 1 \cdot 180° + 320° = -40°$
-40° is correct since we want the car to steer to the right to get to the next focus point!