-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtask_implementations.adb
More file actions
329 lines (300 loc) · 17.8 KB
/
task_implementations.adb
File metadata and controls
329 lines (300 loc) · 17.8 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
package body Task_Implementations is
task body Gyroscope_Reader is
-- Timing Constraints
Next_Period : Ada.Real_Time.Time;
Start_Point : Ada.Real_Time.Time;
Offset : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
Period : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(20);
-- Worst-Case Computation Time Analysis
Execution_Start : Ada.Real_Time.Time;
Execution_End : Ada.Real_Time.Time;
Total_Computation_Time : Ada.Real_Time.Time_Span;
Total_Computation_Time_Limit: Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(5);
Worst_Case_Computation_Time : Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
-- Task Specific Variable Declarations
Velocity : Float;
--Exception variables
Velocity_Max : Float := 6.0;
Velocity_Min : Float := -6.0;
Recoveryblock_Count : Integer := 0;
begin
Motor_Setup.Calibrate_Motors_If_Required;
Epoch.Get_Start_Time(Start_Point);
-- Define next time to run
Next_Period := Start_Point + Offset;
loop
begin
-- Wait for new period
delay until Next_Period;
-- START OF EXECUTION
Execution_Start := Ada.Real_Time.Clock;
Meassure_Velocity.Retrieve_Velocity(Velocity);
Velocity := To_Radians(Velocity);
Gyroscope_SR.Set(Velocity);
-- Define next time to run
Next_Period := Next_Period + Period;
-- END OF EXECUTION
Execution_End := Ada.Real_Time.Clock;
-- Calculation of Total Computation Time
Total_Computation_Time := Execution_End - Execution_Start;
-- Set a new worst-case if current execution time is the largest
if Total_Computation_Time > Worst_Case_Computation_Time then
Worst_Case_Computation_Time := Total_Computation_Time;
end if;
Ada.Text_IO.Put_Line("A" & Duration'Image(To_Duration(Worst_Case_Computation_Time)));
-- Acceptance Test
Acceptance_Test(Velocity_Max, Velocity_Min, Velocity, Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
exception
when Value_Exceed_Max =>
Recoveryblock_Count := Recoveryblock_Count + 1;
Velocity := 3.0;
Gyroscope_SR.Set(Velocity);
Acceptance_Test(Velocity_Max, Velocity_Min, Velocity, Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
when Value_Exceed_Min =>
Recoveryblock_Count := Recoveryblock_Count + 1;
Velocity := -3.0;
Gyroscope_SR.Set(Velocity);
Acceptance_Test(Velocity_Max, Velocity_Min, Velocity, Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
when Excecution_Time_Overun =>
ada.Text_IO.Put_Line("Gyroscope read: Execution time error");
end;
end loop;
end Gyroscope_Reader;
task body Accelerometer_Reader is
-- Timing Constraints
Offset : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
Period : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(20);
Next_Period : Ada.Real_Time.Time;
Start_Point : Ada.Real_Time.Time;
-- I/O Jitter Constraints Of Accelorometer
Next_Reading : Ada.Real_Time.Time;
Read_Period : Ada.Real_Time.Time_Span := Ada.Real_Time.Microseconds(100);
-- Worst-Case Computation Time Analysis
Execution_Start : Ada.Real_Time.Time;
Execution_End : Ada.Real_Time.Time;
Total_Computation_Time : Ada.Real_Time.Time_Span;
Total_Computation_Time_Limit: Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(5);
Worst_Case_Computation_Time : Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
-- Task Specific Variable Declarations
Angle : Float;
-- Summation Of Meassured Coordinates
Sum_Accelerometer_Xvalues : Float := 0.0;
Sum_Accelerometer_Yvalues : Float := 0.0;
-- Average Meassured Value For Each Coordinate
Avg_Accelerometer_Xvalue : Float := 0.0;
Avg_Accelerometer_Yvalue : Float := 0.0;
-- New Meassurement For Each Coordinate
Acceleration_X : Float;
Acceleration_Y : Float;
-- Current Number Of Meassurements And Maximum Number Of Meassurements
Count_Accelerometer_Reads : Natural := 0;
Max_Accelerometer_Reads : Natural := 5;
-- Indicates When Meassuring Is Done According To The Max Counts
Done : Boolean;
-- Exception variables
Angle_Max : Float := 6.0;
Angle_Min : Float := -6.0;
Recoveryblock_Count : Integer := 0;
begin
Motor_Setup.Calibrate_Motors_If_Required;
Epoch.Get_Start_Time(Start_Point);
-- Define next time to run
Next_Period := Start_Point + Offset;
loop
begin
-- Wait for new period
delay until Next_Period;
Execution_Start := Ada.Real_Time.Clock;
-- START OF EXECUTION
Done := False;
-- Define next time to read from sensor (jitter)
Next_Reading := Next_Period;
-- Loop over number of meassurements needed to find the average
loop
delay until Next_Reading;
-- Count is zero when number of readings is equal to the value of max count
Count_Accelerometer_Reads := (Count_Accelerometer_Reads + 1) mod Max_Accelerometer_Reads;
-- Read from sensor
Meassure_Acceleration.Retrieve_Acceleration(Acceleration_X, Acceleration_Y);
-- Adde reading to the sum
Sum_Accelerometer_Xvalues := Sum_Accelerometer_Xvalues + Acceleration_X;
Sum_Accelerometer_Yvalues := Sum_Accelerometer_Yvalues + Acceleration_Y;
-- Check when number of readings is sufficient
if Count_Accelerometer_Reads = 0 then
Done := True;
-- Calculate the average values
Avg_Accelerometer_Xvalue := Sum_Accelerometer_Xvalues / Float(Max_Accelerometer_Reads);
Avg_Accelerometer_Yvalue := Sum_Accelerometer_Yvalues / Float(Max_Accelerometer_Reads);
-- Calculate the angle
Angle := Find_Angle(Avg_Accelerometer_Xvalue, Avg_Accelerometer_Yvalue);
-- Store the angle in a shared protected object
Accelerometer_SR.Set(Angle);
-- Reset
Sum_Accelerometer_Xvalues := 0.0;
Sum_Accelerometer_Yvalues := 0.0;
end if;
-- Exit when we have found an average sensor value
exit when Done;
-- Define the next time to run (Period T)
Next_Reading := Next_Reading + Read_Period;
end loop;
Next_Period := Next_Period + Period;
-- END OF EXECUTION
Execution_End := Ada.Real_Time.Clock;
-- Calculation of Total Computation Time
Total_Computation_Time := Execution_End - Execution_Start;
-- Set a new worst-case if current execution time is the largest
if Total_Computation_Time > Worst_Case_Computation_Time then
Worst_Case_Computation_Time := Total_Computation_Time;
end if;
Ada.Text_IO.Put_Line("B" & Duration'Image(To_Duration(Worst_Case_Computation_Time)));
-- Acceptance Test
Acceptance_Test(Angle_Max, Angle_Min, Angle, Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
exception
when Value_Exceed_Max =>
Recoveryblock_Count := Recoveryblock_Count + 1;
Angle := 0.01;
Accelerometer_SR.Set(Angle);
Acceptance_Test(Angle_Max, Angle_Min, Angle, Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
when Value_Exceed_Min =>
Recoveryblock_Count := Recoveryblock_Count + 1;
Angle := -0.01;
Accelerometer_SR.Set(Angle);
Acceptance_Test(Angle_Max, Angle_Min, Angle, Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
when Excecution_Time_Overun =>
Ada.Text_IO.Put_Line("Accelerometer read: Execution time error");
end;
end loop;
end Accelerometer_Reader;
task body Cascade_Controller is
-- Timing Constraints
Offset : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
Period : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(20);
Next_Period : Ada.Real_Time.Time;
Start_Point : Ada.Real_Time.Time;
-- Worst-Case Computation Time Analysis
Execution_Start : Ada.Real_Time.Time;
Execution_End : Ada.Real_Time.Time;
Total_Computation_Time : Ada.Real_Time.Time_Span;
Total_Computation_Time_Limit: Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(5);
Worst_Case_Computation_Time : Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
-- Exception Detection Variables
Actuator_Value_Max : Float := 2000.0;
Actuator_Value_Min : Float := 1000.0;
Recoveryblock_Count : Integer := 0;
-- Task Specific Variable Declarations
Current_Angle : Float;
Current_Velocity : Float;
Actuator_Value : RPM;
Motor_To_Run : Motor_Direction;
-- PID Variables
Position_Reference : Constant Float := 0.0;
Velocity_Reference : Float;
Torque_Reference : Float;
Position_Error : Float;
Velocity_Error : Float;
K_PP : Constant Float := 1.0;
K_PV : Constant Float := 10.0;
begin
Motor_Setup.Calibrate_Motors_If_Required;
Epoch.Get_Start_Time(Start_Point);
-- Define next time to run
Next_Period := Start_Point + Offset;
loop
begin
delay until Next_Period; -- Wait for new period
-- START OF EXECUTION
Execution_Start := Ada.Real_Time.Clock;
-- Get current angle from accelerometer
Gyroscope_SR.Get(Current_Velocity);
Accelerometer_SR.Get(Current_Angle);
Position_Error := Position_Reference - Current_Angle;
Position_Controller(K_PP, Position_Error, Velocity_Reference);
-- Get current angle from gyroscope
Velocity_Error := Velocity_Reference - Current_Velocity;
Velocity_Controller(K_PV, Velocity_Error, Torque_Reference);
Map_To_RPM(Torque_Reference, Actuator_Value);
-- Decide which motor to be activated
if Torque_Reference > 0.0 then
Motor_To_Run := Right;
else
Motor_To_Run := Left;
end if;
-- Set RPM value to protected object
Motor_AW.Set(Actuator_Value, Motor_To_Run);
-- Define next time to run
Next_Period := Next_Period + Period;
-- END OF EXECUTION
Execution_End := Ada.Real_Time.Clock;
-- Calculation of Total Computation Time
Total_Computation_Time := Execution_End - Execution_Start;
-- Set a new worst-case if current execution time is the largest
if Total_Computation_Time > Worst_Case_Computation_Time then
Worst_Case_Computation_Time := Total_Computation_Time;
end if;
Ada.Text_IO.Put_Line("C" & Duration'Image(To_Duration(Worst_Case_Computation_Time)));
-- Acceptance Test
Acceptance_Test(Actuator_Value_Max, Actuator_Value_Min, Float(Actuator_Value), Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
exception
when Value_Exceed_Max =>
Recoveryblock_Count := Recoveryblock_Count + 1;
Actuator_Value := 2000;
Motor_AW.Set(Actuator_Value, Motor_To_Run);
Acceptance_Test(Actuator_Value_Max, Actuator_Value_Min, Float(Actuator_Value), Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
when Value_Exceed_Min =>
Recoveryblock_Count := Recoveryblock_Count + 1;
Actuator_Value := 1000;
Motor_AW.Set(Actuator_Value, Motor_To_Run);
Acceptance_Test(Actuator_Value_Max, Actuator_Value_Min, Float(Actuator_Value), Total_Computation_Time, Total_Computation_Time_Limit, Recoveryblock_Count);
when Excecution_Time_Overun =>
Ada.Text_IO.Put_Line("Cascade controller: Execution time error");
end;
end loop;
end Cascade_Controller;
task body Actuator_Writer is
-- Timing Constraints
Offset : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
Period : constant Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(20);
Next_Period : Ada.Real_Time.Time;
Start_Point : Ada.Real_Time.Time;
-- We do not wish to write to the motor to early
Min_Start_Time : Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(15);
-- Worst-Case Computation Time Analysis
Execution_Start : Ada.Real_Time.Time;
Execution_End : Ada.Real_Time.Time;
Total_Computation_Time : Ada.Real_Time.Time_Span;
Worst_Case_Computation_Time : Ada.Real_Time.Time_Span := Ada.Real_Time.Milliseconds(0);
-- Task Specific Variable Declarations
Actuator_Value : RPM;
Motor_To_Run : Motor_Direction;
begin
Motor_Setup.Calibrate_Motors_If_Required;
Epoch.Get_Start_Time(Start_Point);
-- Define next time to run
Next_Period := Start_Point + Offset;
loop
-- Wait for new period
delay until Next_Period + Min_Start_Time;
-- START OF EXECUTION
Execution_Start := Ada.Real_Time.Clock;
Motor_AW.Get(Actuator_Value, Motor_To_Run);
if Motor_To_Run = Right then
Write(3,Actuator_Value);
Set_Analog_Period_Us(20000);
else
Write(28,Actuator_Value);
Set_Analog_Period_Us(20000);
end if;
Next_Period := Next_Period + Period;
-- END OF EXECUTION
Execution_End := Ada.Real_Time.Clock;
-- Calculation of Total Computation Time
Total_Computation_Time := Execution_End - Execution_Start;
-- Set a new worst-case if current execution time is the largest
if Total_Computation_Time > Worst_Case_Computation_Time then
Worst_Case_Computation_Time := Total_Computation_Time;
end if;
Ada.Text_IO.Put_Line("D" & Duration'Image(To_Duration(Worst_Case_Computation_Time)));
end loop;
end Actuator_Writer;
end Task_Implementations;