ROSCOQ.robots.ackermannSteering

Require Export Coq.Program.Tactics.
Require Export LibTactics.


Require Import Vector.
Require Import CPS.
Require Import CPSUtils.

Require Import MathClasses.interfaces.canonical_names.
Require Import MCInstances.
Require Import CartCR.

Require Export CartIR.

Notation FConst := ConstTContR.
Notation FSin:= CFSine.
Notation FCos:= CFCos.

TContR is a type of continuous functions from Time to reals (R). It denotes how a physical quantity evolved over time. Recall that Time is a type of non-negative reals. For a detailed example, see Sec. 2 of the ROSCoq paper.
Also, see the ROSCoq wiki for a more complete example of how robots are specified in ROSCoq. This file is experimental.

Local Notation "Time -c-> R" := TContR (at level 100).

Ackermann Steering

The geometry of Ackermann steering is illustrated in the figure below:
The key idea is that while turning, the rotation axes of all 4 wheels meet at a point, which is denoted by turnCenter in the figure. This point must lie on the line joining the rear wheels, because they are fixed relative to the car. While turning, the whole car rotates around this point. Note that while turning the front wheels are NOT parallel to each other.

Section Robot.

Because of the limited range of the steering wheel, turn radius cannot be made arbitrary small. Thus, the turn curvature, which is its inverse, cannot be made arbitrary large

Variable maxTurnCurvature : Qpos.

Set Implicit Arguments.

Record AckermannCar : Type := {

instantaneous linear velocity of the midpoint of the 2 rear wheels

  linVel : (Time -c R);

We also need to model the position of the turning center. As mentined and illustrated in the figure above, we know that it lies on the line joining the 2 rear wheels. One way to model it is to have a physical quantity denoting the turn radius (at time t), which is the displacement from the midpoint from the 2 wheels, along that line. A positive value indicates that the turn center is on the left side of the car.
However, this turn radius is a poorly behaved function. When one moves the steering wheel from a little left of the midpoint to a little right, the turn radious goes discontinuously from a very large negative value, to undefined, to a very large positive value.
Note that the turn radius is always larger than a positive quantity, because of physical constraints. Hence, we can model its (multiplicative) inverse, which can be understood as "curvature". Curvature is much more well-behaved. During the above process, it goes continuously from a small negative value to 0 to a small positive value.

  turnCurvature : (Time -c R);



The above two physical quantities are the only controllable ones for the car. linVel is controlled via the gears, gas pedal and the brake pedal. turnCurvature is coltrolled via the steering wheel.
Position of the midpoint of the 2 rear wheels

  position :> Cart2D (Time -c R);

The angle (w.r.t X axis) made by the the line fron the the midpoint of the back of the car to the midpoint of the front of the car
  theta : (Time -c R);

  
differential equations
w = v/r. Recall that curvature = 1/r, where r is the turn radius.
length of the car w.r.t. the line joining the rear wheels

Variable lengthFront : Q.
Variable lengthBack : Q.

width of the car

Variable width :Q.

End Robot.