Skip to content

Refactor controller #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
manthonyaiello wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Refactor controller #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
47b2dd9
Fix project file: remove backslash in path.
manthonyaiello Jul 25, 2022
91fae69
Refactor the steering controller.
manthonyaiello Jul 25, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion rc_car.gpr
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ project RC_Car extends "Ada_Drivers_Library/examples/shared/common/common.gpr" i

for Languages use ("Ada");

for Source_Dirs use ("src\**",
for Source_Dirs use ("src/**",
"../Robotics_with_Ada/src/**");

for Object_Dir use "obj/" & App_BUILD;
Expand Down
136 changes: 136 additions & 0 deletions src/ada_steering_controller.adb
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,136 @@
with Steering_Control;
with Vehicle;

package body Ada_Steering_Controller with
SPARK_Mode
is
-- steering PID gain constants
Kp : constant := 6.0;
Ki : constant := 5.0;
Kd : constant := 0.1;

Power_Level_Last : constant Float := Float (Power_Level'Last);

Power_Level_Limits : constant Closed_Loop.Bounds :=
(Min => -Power_Level_Last, Max => +Power_Level_Last);
-- The limits for the PID controller output power values, based on the
-- NXT motor's largest power value. The NXT Power_Level type is an integer
-- ranging from 0 to 100. The PID controller wil compute negative values
-- as well as posiitve values, to turn the steering mechanism in either
-- direction, so we want to limit the PID to -100 .. 100. We later take the
-- absolute value after using the sign to get the direction, in procedure
-- Convert_To_Motor_Values.

function Convert_To_Motor_Angle (Encoder_Count : Motor_Encoder_Counts) return Float with Inline;
-- Returns the current encoder count for This motor in degree units

procedure Convert_To_Motor_Values
(Signed_Power : Float;
Motor_Power : out NXT.Motors.Power_Level;
Direction : out NXT.Motors.Directions)
with
Inline;
-- Convert the signed power value from the PID controller to NXT motor
-- values. We know the precondition will hold because we set those limits
-- via the call to Steering_Computer.Configure

Steering_Offset : Float;
-- The angle at which the vehicle is steering straight ahead. Hard
-- right is always 0 and hard left is always Steering_Offset * 2.
-- This value is used to convert from the "observer's" frame of
-- reference to the vehicle's frame of reference.

---------------
-- Configure --
---------------

procedure Configure (Steering_Offset : Float) is
begin
Ada_Steering_Controller.Steering_Offset := Steering_Offset;
end Configure;

----------------
-- Initialize --
----------------

procedure Initialize (State : in out Ada_Steering_Controller_State)
is
begin
State.Steering_Computer.Configure
(Proportional_Gain => Kp,
Integral_Gain => Ki,
Derivative_Gain => Kd,
Period => 10, -- 10 ms = 100Hz = 0.01 seconds
Output_Limits => Power_Level_Limits,
Direction => Closed_Loop.Direct);

State.Current_Angle := 0.0; -- zero for call to Steering_Computer.Enable
State.Steering_Power := 0.0; -- zero for call to Steering_Computer.Enable

State.Steering_Computer.Enable (
Process_Variable => State.Current_Angle,
Control_Variable => State.Steering_Power);
end Initialize;

-------------
-- Compute --
-------------

procedure Compute
(Encoder_Count : Motor_Encoder_Counts;
Requested_Steering_Angle : Integer_8;
Motor_Power : out Power_Level;
Rotation_Direction : out Directions;
State : in out Ada_Steering_Controller_State)
is
Target_Angle : Float;
begin
State.Current_Angle := Convert_To_Motor_Angle (Encoder_Count) - Steering_Offset;

Target_Angle := Float (Requested_Steering_Angle);

if Target_Angle < -Steering_Offset then
Target_Angle := -Steering_Offset;
elsif Target_Angle > Steering_Offset then
Target_Angle := Steering_Offset;
end if;

State.Steering_Computer.Compute_Output
(Process_Variable => State.Current_Angle,
Setpoint => Target_Angle,
Control_Variable => State.Steering_Power);

Convert_To_Motor_Values (State.Steering_Power, Motor_Power, Rotation_Direction);
end Compute;


function Within_Limits (State : Ada_Steering_Controller_State) return Boolean is
((State.Steering_Computer.Current_Output_Limits = Power_Level_Limits) and
(Closed_Loop.Within_Limits (State.Steering_Power, Power_Level_Limits)));


-------------------------
-- Convert_To_Motor_Angle --
-------------------------

function Convert_To_Motor_Angle (Encoder_Count : Motor_Encoder_Counts) return Float is
(Float (Encoder_Count) / Float (Steering_Control.Encoder_Counts_Per_Degree));


-----------------------------
-- Convert_To_Motor_Values --
-----------------------------

procedure Convert_To_Motor_Values
(Signed_Power : Float;
Motor_Power : out NXT.Motors.Power_Level;
Direction : out NXT.Motors.Directions)
is
begin
Direction := Vehicle.To_Steering_Motor_Direction (Signed_Power);
-- The motor values are a percentage from 0 .. 100. The sign of the value
-- is only used in the statement above, for "turn" directions.
Motor_Power := Power_Level (abs (Signed_Power));
end Convert_To_Motor_Values;

end Ada_Steering_Controller;
39 changes: 39 additions & 0 deletions src/ada_steering_controller.ads
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
with Interfaces; use Interfaces;

with NXT.Motors; use NXT.Motors;

with Process_Control_Floating_Point;

package Ada_Steering_Controller with
SPARK_Mode
is
type Ada_Steering_Controller_State is tagged limited private;

procedure Configure (Steering_Offset : Float);
-- Configure the Ada steering controller.

procedure Initialize (State : in out Ada_Steering_Controller_State);
-- Initialize the states for the Ada controller.

procedure Compute (
Encoder_Count : Motor_Encoder_Counts;
Requested_Steering_Angle : Integer_8;
Motor_Power : out Power_Level;
Rotation_Direction : out Directions;
State : in out Ada_Steering_Controller_State);
-- Compute control outputs based on inputs and state. Note: this also
-- updates state.

function Within_Limits (State : Ada_Steering_Controller_State) return Boolean
with Inline;

private
package Closed_Loop is new Process_Control_Floating_Point (Float, Long_Float);
use Closed_Loop;

type Ada_Steering_Controller_State is tagged limited record
Steering_Computer : Closed_Loop.PID_Controller;
Current_Angle : Float;
Steering_Power : Float;
end record;
end Ada_Steering_Controller;
104 changes: 22 additions & 82 deletions src/steering_control.adb
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
with Global_Initialization;
with Ada.Real_Time; use Ada.Real_Time;
with Interfaces; use Interfaces;
with Vehicle; use Vehicle;
with NXT.Motors; use NXT.Motors;
with Remote_Control;
with Math_Utilities;
with Process_Control_Floating_Point;
with Ada_Steering_Controller;

package body Steering_Control
with SPARK_Mode
Expand All @@ -13,91 +13,48 @@ is
Period : constant Time_Span := Milliseconds (System_Configuration.Steering_Control_Period);
-- NB: important to PID tuning!

package Closed_Loop is new Process_Control_Floating_Point (Float, Long_Float);
use Closed_Loop;

-- steering PID gain constants
Kp : constant := 6.0;
Ki : constant := 5.0;
Kd : constant := 0.1;

Power_Level_Last : constant Float := Float (Power_Level'Last);

Power_Level_Limits : constant Closed_Loop.Bounds :=
(Min => -Power_Level_Last, Max => +Power_Level_Last);
-- The limits for the PID controller output power values, based on the
-- NXT motor's largest power value. The NXT Power_Level type is an integer
-- ranging from 0 to 100. The PID controller wil compute negative values
-- as well as posiitve values, to turn the steering mechanism in either
-- direction, so we want to limit the PID to -100 .. 100. We later take the
-- absolute value after using the sign to get the direction, in procedure
-- Convert_To_Motor_Values.

Encoder_Counts_Per_Degree : constant Float := Float (NXT.Motors.Encoder_Counts_Per_Revolution) / 360.0;

procedure Limit is new Math_Utilities.Bound_Floating_Value (Float);

procedure Initialize_Steering_Mechanism (Center_Offset : out Float) with
Post => Center_Offset > 0.0;
-- Compute the steering zero offset value by powering the steering mechanism
-- to the mechanical limits.

function Current_Motor_Angle (This : Basic_Motor) return Float with Inline;
-- Returns the current encoder count for This motor in degree units

procedure Convert_To_Motor_Values
(Signed_Power : Float;
Motor_Power : out NXT.Motors.Power_Level;
Direction : out NXT.Motors.Directions)
with
Inline,
Pre => Within_Limits (Signed_Power, Power_Level_Limits);
-- Convert the signed power value from the PID controller to NXT motor
-- values. We know the precondition will hold because we set those limits
-- via the call to Steering_Computer.Configure
-- Returns the current angle of This motor in degree units

-----------
-- Servo --
-----------

task body Servo is
Controller_State : Ada_Steering_Controller.Ada_Steering_Controller_State;
Next_Release : Time;
Target_Angle : Float;
Current_Angle : Float := 0.0; -- zero for call to Steering_Computer.Enable
Steering_Power : Float := 0.0; -- zero for call to Steering_Computer.Enable
Target_Angle : Integer_8;
Motor_Power : NXT.Motors.Power_Level;
Rotation_Direction : NXT.Motors.Directions;
Steering_Offset : Float;
Steering_Computer : Closed_Loop.PID_Controller;
begin
Steering_Computer.Configure
(Proportional_Gain => Kp,
Integral_Gain => Ki,
Derivative_Gain => Kd,
Period => System_Configuration.Steering_Control_Period,
Output_Limits => Power_Level_Limits,
Direction => Closed_Loop.Direct);

Global_Initialization.Critical_Instant.Wait (Epoch => Next_Release);

Initialize_Steering_Mechanism (Steering_Offset);
declare
Steering_Offset : Float;
begin
Initialize_Steering_Mechanism (Steering_Offset);

Steering_Computer.Enable (Process_Variable => Current_Angle, Control_Variable => Steering_Power);
loop
pragma Loop_Invariant (Steering_Computer.Current_Output_Limits = Power_Level_Limits);
pragma Loop_Invariant (Within_Limits (Steering_Power, Power_Level_Limits));
Ada_Steering_Controller.Configure (Steering_Offset);
end;

Current_Angle := Current_Motor_Angle (Steering_Motor) - Steering_Offset;
Ada_Steering_Controller.Initialize (Controller_State);

Target_Angle := Float (Remote_Control.Requested_Steering_Angle);
Limit (Target_Angle, -Steering_Offset, Steering_Offset);
loop
pragma Loop_Invariant (Ada_Steering_Controller.Within_Limits (Controller_State));

Steering_Computer.Compute_Output
(Process_Variable => Current_Angle,
Setpoint => Target_Angle,
Control_Variable => Steering_Power);
Target_Angle := Integer_8 (Remote_Control.Requested_Steering_Angle);

Convert_To_Motor_Values (Steering_Power, Motor_Power, Rotation_Direction);
Ada_Steering_Controller.Compute
(Encoder_Count => Steering_Motor.Encoder_Count,
Requested_Steering_Angle => Target_Angle,
Motor_Power => Motor_Power,
Rotation_Direction => Rotation_Direction,
State => Controller_State);

Steering_Motor.Engage (Rotation_Direction, Motor_Power);

Expand All @@ -106,28 +63,12 @@ is
end loop;
end Servo;

-----------------------------
-- Convert_To_Motor_Values --
-----------------------------

procedure Convert_To_Motor_Values
(Signed_Power : Float;
Motor_Power : out NXT.Motors.Power_Level;
Direction : out NXT.Motors.Directions)
is
begin
Direction := Vehicle.To_Steering_Motor_Direction (Signed_Power);
-- The motor values are a percentage from 0 .. 100. The sign of the value
-- is only used in the statement above, for "turn" directions.
Motor_Power := Power_Level (abs (Signed_Power));
end Convert_To_Motor_Values;

-------------------------
-- Current_Motor_Angle --
-------------------------

function Current_Motor_Angle (This : Basic_Motor) return Float is
((Float (This.Encoder_Count) / Encoder_Counts_Per_Degree));
(Float (This.Encoder_Count) / Float (Encoder_Counts_Per_Degree));

-----------------------------------
-- Initialize_Steering_Mechanism --
Expand Down Expand Up @@ -253,5 +194,4 @@ is
-- from the motor's reported angle to translate the reported value into
-- the vehicle's frame of reference.
end Initialize_Steering_Mechanism;

end Steering_Control;
5 changes: 5 additions & 0 deletions src/steering_control.ads
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,16 @@
with System_Configuration;
with NXT.Motors;

package Steering_Control
with SPARK_Mode
is

pragma Elaborate_Body;

Encoder_Counts_Per_Degree : constant NXT.Motors.Motor_Encoder_Counts :=
NXT.Motors.Motor_Encoder_Counts (Float (NXT.Motors.Encoder_Counts_Per_Revolution) / 360.0);
-- The number of encoder values reported per degree of rotation.

private

task Servo with
Expand Down