def test_speed_power_more_than_max_set(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 = 101
# WHEN
# THEN
self.assertRaises(Exception, truck.setSpeedPower, power_value)
self.assertRaises(Exception, truck.setSpeedPower, -power_value)
def test_stop(self):
# GIVEN
left_motor_mock = self.create_mock_motor()
right_motor_mock = self.create_mock_motor()
truck = Truck(left_motor_mock, right_motor_mock)
# WHEN
truck.stop()
# THEN
left_motor_mock.stop.assert_called()
right_motor_mock.stop.assert_called()
def test_speed_power_zero_set(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
# WHEN
truck.setSpeedPower(power_value)
# THEN
left_motor_mock.stop.assert_called()
right_motor_mock.stop.assert_called()
def test_speed_power_negative_set(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
# WHEN
truck.setSpeedPower(power_value)
# THEN
left_motor_mock.backward.assert_called_with(-power_value)
right_motor_mock.backward.assert_called_with(-power_value)
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)
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
from alphabot.hardware.motor_module import LeftMotor, RightMotor
from alphabot.truck_module import Truck
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)
