calculate elapsed_time and average_velocity

src/hello_world/__init__.py

@@ -9,33 +9,44 @@ class Orientation(NamedTuple):
     theta_radians: float
     theta_degrees: float
 
+class DeltaTime(NamedTuple):
+    current_time: int
+    target_time: int
+
 current_pos = Vector(3,2)
 target_pos = Vector(7,5)
 zero_vector = Vector(0,0)
+delta_time = DeltaTime(2, 5)
+
 
 def main() -> None:
     print("Hello Teodora!")
 
     displacement = displacementVector(current_pos, target_pos) 
-    print(displacement)
+    print("displacemnt = ", displacement)
 
     distance =  length(displacement)
-    print(distance)
+    print("distance = ", distance)
 
     direction = safe_normalize(displacement, 0.001)  # minimum distance = 0.001 or 1.0e-6 
-                                                    # the safe guard depends on the our coordinate scale (ex. cm or m)
-    print(direction)
+                                                     # the safe guard depends on our coordinate scale (ex. cm or m)
+    print("direction = ", direction)
 
     theta = orientation(displacement)
-    print(theta)
+    print("theta = ", theta)
 
     #phase = wrap_phase(theta.theta_radians)
     phase = wrap_phase(6.5)
-    print(phase) 
+    print("phase = ", phase) 
 
     phase = wrap_phase_v2(730)
-    print(phase)
+    print("Phase_2 = ", phase)
 
+    elapsed_time = fct_elapsed_time(delta_time)
+    print("elapsed_time = ", elapsed_time)
+
+    average_velocity = fct_average_velocity(displacement, elapsed_time)
+    print("average_Velocity = ", average_velocity)
 
 
 ## calculate the displacement from current position to target position
@@ -92,9 +103,21 @@ def wrap_phase_v2(phase: int) -> int:
     return phase
 
 
+## signed phase difference
+    # This function calculates the shortest signed angular difference from one orientation to another.
+
+#def signed_difference(from_phase: float, to_phase: float) -> float:
+   # shifted = to_phase - from_phase + math.pi
+   # wrapped = wrap_phase(shifted)
+   # return wrapped - math.pi
 
 
+## velocity: rate of cange of position with respect to time
 
+def fct_elapsed_time(delta_time: DeltaTime) ->int:
+    elapsed_time = delta_time.target_time - delta_time.current_time
+    return elapsed_time
 
-
- 
+def fct_average_velocity(displacement: Vector, elapsed_time: int) -> Vector:
+    upsilon = Vector(displacement.x / elapsed_time , displacement.y / elapsed_time)
+    return upsilon