diff --git a/rc_car.gpr b/rc_car.gpr index 0efb82d..ae53389 100644 --- a/rc_car.gpr +++ b/rc_car.gpr @@ -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; diff --git a/src/ada_steering_controller.adb b/src/ada_steering_controller.adb new file mode 100644 index 0000000..e97ccef --- /dev/null +++ b/src/ada_steering_controller.adb @@ -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; \ No newline at end of file diff --git a/src/ada_steering_controller.ads b/src/ada_steering_controller.ads new file mode 100644 index 0000000..d31f85b --- /dev/null +++ b/src/ada_steering_controller.ads @@ -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; \ No newline at end of file diff --git a/src/steering_control.adb b/src/steering_control.adb index 6b38c58..a1a0230 100644 --- a/src/steering_control.adb +++ b/src/steering_control.adb @@ -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 @@ -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); @@ -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 -- @@ -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; diff --git a/src/steering_control.ads b/src/steering_control.ads index f5654f0..6b4f0c4 100644 --- a/src/steering_control.ads +++ b/src/steering_control.ads @@ -1,4 +1,5 @@ with System_Configuration; +with NXT.Motors; package Steering_Control with SPARK_Mode @@ -6,6 +7,10 @@ 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