def test_speed_power_positive_with_turn_power_negative_small(self):
# GIVEN
left_motor_mock = self.create_mock_motor()
right_motor_mock = self.create_mock_motor()
truck = Truck(left_motor_mock, right_motor_mock)
power_value = 10
truck.setSpeedPower(power_value)
# WHEN
truck.setTurnPower(-20)
# THEN
left_motor_mock.forward.assert_called_with(10)
right_motor_mock.forward.assert_called_with(30)
def test_speed_power_zero_with_turn_power_negative_big(self):
# GIVEN
left_motor_mock = self.create_mock_motor()
right_motor_mock = self.create_mock_motor()
truck = Truck(left_motor_mock, right_motor_mock)
power_value = 0
truck.setSpeedPower(power_value)
# WHEN
truck.setTurnPower(-100)
# THEN
left_motor_mock.backward.assert_called_with(50)
right_motor_mock.forward.assert_called_with(50)
logger = logging.getLogger(__name__)
logging.basicConfig(level=logging.INFO)
gpio = GpioWrapper()
sensors_adc = LineSensorsAdc(GpioWrapper())
telemetry = Telemetry()
prevent_time = None
current_time_before_all = time.time_ns()
speed_power = 0
truck = Truck(LeftMotor(gpio), RightMotor(gpio))
truck.setSpeedPower(speed_power)
truck.setTurnPower(50)
for i in range(0, 5000):
all_sensors_values = sensors_adc.readSensors()
current_time = time.time_ns()
if prevent_time is None:
prevent_time = current_time
continue
delta_time_ms = (current_time - prevent_time) / 1_000_000
prevent_time = current_time
telemetry_item = {'dt': delta_time_ms, 'sns': all_sensors_values}
telemetry.send(telemetry_item)
logging.info(telemetry_item)
time.sleep(0.001)
truck.setTurnPower(0)
truck.setSpeedPower(0)
class LineFollower:
def __init__(self,
config: LineFollowerConfig = LineFollowerConfig(),
gpio: GpioWrapper = None):
self._cfg = config
self._sensor: LineSensor = LineSensorNormalizer(
LineSensorFilter(LineSensorsAdc(gpio)))
self._logger = logging.getLogger(__name__)
self._speed_power = config.SPEED_POWER
self._sleep_time = config.SLEEP_TIME
self._left_sensor_pid = PidController(config.KP, config.KI, config.KD,
config.TARGET_VALUE_LEFT,
config.MAX_OUT)
self._right_sensor_pid = PidController(config.KP, config.KI, config.KD,
config.TARGET_VALUE_RIGHT,
config.MAX_OUT)
self._bot_truck = Truck(LeftMotor(gpio), RightMotor(gpio))
self._telemetry = Telemetry()
self._prevent_time_ns = None
self._keep_following = False
def startFollowing(self):
self._sleepAndMeasureTime()
self._keep_following = True
while self._keep_following:
delta_time = self._sleepAndMeasureTime()
all_sensors_values = self._sensor.readSensors()
self._doFollowingAlgorythm(all_sensors_values, delta_time)
self._bot_truck.stop()
def _doFollowingAlgorythm(self, all_sensors_values, delta_time):
self._correctCourse(all_sensors_values, delta_time)
self._sendTelemetry(all_sensors_values, delta_time)
self._stopFollowingWhenBotIsOutOfLine(all_sensors_values)
def _stopFollowingWhenBotIsOutOfLine(self, all_sensors_values: List):
if self._isBotOutOfLine(all_sensors_values):
self._keep_following = False
def _correctCourse(self, all_sensors_values: List, delta_time):
self._bot_truck.setSpeedPower(self._speed_power)
self._bot_truck.setTurnPower(
self._calculateTurnPower(delta_time, all_sensors_values))
def _calculateTurnPower(self, delta_time: float, all_sensors_values: List):
left_sensor_value = all_sensors_values[1]
right_sensor_value = all_sensors_values[3]
left_sensor_pid_out = self._left_sensor_pid.getOutput(
left_sensor_value, delta_time)
right_sensor_pid_out = self._right_sensor_pid.getOutput(
right_sensor_value, delta_time)
if left_sensor_pid_out is None or right_sensor_pid_out is None:
return 0
if left_sensor_pid_out < 0 and right_sensor_pid_out < 0:
if left_sensor_pid_out < right_sensor_pid_out:
right_sensor_pid_out = 0
elif right_sensor_pid_out < left_sensor_pid_out:
left_sensor_pid_out = 0
if left_sensor_pid_out < 0:
return -left_sensor_pid_out
if right_sensor_pid_out < 0:
return right_sensor_pid_out
return 0
def _sleepAndMeasureTime(self):
time.sleep(self._sleep_time)
delta_time_ns = self._calculateDeltaTimeInNanos()
if delta_time_ns is None:
return None
return self._to_ms(delta_time_ns)
def _calculateDeltaTimeInNanos(self):
current_time_ns = time.time_ns()
if self._prevent_time_ns is None:
self._prevent_time_ns = current_time_ns
return None
delta_time_ns = (current_time_ns - self._prevent_time_ns)
self._prevent_time_ns = current_time_ns
return delta_time_ns
def _to_ms(self, time_ns):
return time_ns / 1_000_000
def _to_mcs(self, time_ns):
return time_ns / 1_000
def _isBotOutOfLine(self, all_sensors_values):
for value in all_sensors_values:
if value not in self._cfg.white_level:
return False
return True
def _sendTelemetry(self, all_sensors_values, delta_time):
self._telemetry.send({
'flv': {
'tm': time.time_ns(),
'dt': delta_time,
'sns': all_sensors_values,
'sp': self._bot_truck.getSpeedPower(),
'tn': self._bot_truck.getTurnPower()
},
'lp': self._left_sensor_pid.getTelemetryData(),
'rp': self._right_sensor_pid.getTelemetryData()
})
@property
def logger(self):
return self._logger