drv/virtual/simulation: Implement initial state.
Give each motor an initial time and random initial angle.
@@ -7,7 +7,9 @@ from ..drv.led import VirtualLed
from ..drv.clock import CountingClock
from ..drv.ioport import VirtualIOPort, PortId, IODeviceTypeId
-from numpy import array
+import numpy
+import random
+import math
from ..physics.motors import SimpleMotor as SimMotor
@@ -39,7 +41,7 @@ class VirtualMotorDriver:
# Simulate the motor up to the current time, plus 5 milliseconds.
end_time = args[0] / 1000000 + 0.005
- u = array([args[1]])
+ u = numpy.array([args[1]])
self.sim_motor.simulate(end_time, u)
@@ -52,13 +54,29 @@ class VirtualCounter:
def __init__(self, sim_motor):
self.sim_motor = sim_motor
+ # Nothing left to do if there is no motor.
+ if self.sim_motor is None:
+ return
+
+ # Since all counter drivers count 0 from the point where it started,
+ # we need to keep track of the starting point.
+ self.initial_angle, speed = self.sim_motor.get_latest_output()
+
@property
def abs_count(self):
"""
Provides the value for ``pbdrv_counter_virtual_get_abs_count()``.
"""
+ # Return 0 if there is no motor.
+ if self.sim_motor is None:
return 0
+ # Return the latest available data from the simulation model.
+ angle, speed = self.sim_motor.get_latest_output()
+
+ abs_angle = angle % 360
+ return abs_angle if abs_angle < 180 else abs_angle - 360
+
@property
def count(self):
"""
@@ -69,9 +87,11 @@ class VirtualCounter:
if self.sim_motor is None:
return 0
- # Return the latest available data from the simulation model.
+ # Get the latest available data from the simulation model.
angle, speed = self.sim_motor.get_latest_output()
- return angle
+
+ # Counter drivers count from the angle where they started.
+ return angle - self.initial_angle
@property
def rate(self):
@@ -123,7 +143,24 @@ class Platform:
self.ioport[port_id].motor_type_id = type_id
# Initialize motor simulation model.
- self.sim_motor[i] = None if type_id == IODeviceTypeId.NONE else SimMotor()
+ self.sim_motor[i] = None
+ if type_id != IODeviceTypeId.NONE:
+
+ # Get current time.
+ initial_time = self.clock[-1].microseconds / 1000000
+
+ # Random initial motor angle with zero speed.
+ initial_angle = random.randint(-180, 179)
+ initial_speed = 0
+ initial_state = numpy.array(
+ [
+ math.radians(initial_angle),
+ math.radians(initial_speed),
+ ]
+ )
+
+ # Initialize simulated motor.
+ self.sim_motor[i] = SimMotor(t0=initial_time, x0=initial_state)
# Initialize counter and motor drivers with the given motor.
self.counter[i] = VirtualCounter(self.sim_motor[i])
