135 lines
4.3 KiB
Python
135 lines
4.3 KiB
Python
"""
|
|
Setup the devices connected to the robot.
|
|
|
|
The main configuration for the devices connected to the robot is the devices
|
|
list in the setup_devices function.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import logging
|
|
|
|
from sbot_interface.boards import (
|
|
Arduino,
|
|
CameraBoard,
|
|
LedBoard,
|
|
MotorBoard,
|
|
PowerBoard,
|
|
ServoBoard,
|
|
TimeServer,
|
|
)
|
|
from sbot_interface.devices.arduino_devices import (
|
|
EmptyPin,
|
|
MicroSwitch,
|
|
ReflectanceSensor,
|
|
UltrasonicSensor,
|
|
)
|
|
from sbot_interface.devices.camera import Camera
|
|
from sbot_interface.devices.led import Led, NullLed
|
|
from sbot_interface.devices.motor import Motor
|
|
from sbot_interface.devices.power import NullBuzzer, Output, StartButton
|
|
from sbot_interface.devices.servo import NullServo
|
|
from sbot_interface.socket_server import Board, DeviceServer, SocketServer
|
|
|
|
|
|
def setup_devices(log_level: int | str = logging.WARNING) -> SocketServer:
|
|
"""
|
|
Setup the devices connected to the robot.
|
|
|
|
Contains the main configuration for the devices connected to the robot.
|
|
|
|
:param log_level: The logging level to use for the device logger.
|
|
:return: The socket server which will handle all connections and commands.
|
|
"""
|
|
device_logger = logging.getLogger('sbot_interface')
|
|
device_logger.setLevel(log_level)
|
|
|
|
# this is the configuration of devices connected to the robot
|
|
devices: list[Board] = [
|
|
PowerBoard(
|
|
outputs=[Output() for _ in range(7)],
|
|
buzzer=NullBuzzer(),
|
|
button=StartButton(),
|
|
leds=(NullLed(), NullLed()),
|
|
asset_tag='PWR',
|
|
),
|
|
MotorBoard(
|
|
motors=[
|
|
Motor('left motor'),
|
|
Motor('right motor'),
|
|
],
|
|
asset_tag='MOT',
|
|
),
|
|
ServoBoard(
|
|
servos=[NullServo() for _ in range(8)],
|
|
asset_tag='SERVO',
|
|
),
|
|
LedBoard(
|
|
leds=[
|
|
Led('led 1', num_colours=8),
|
|
Led('led 2', num_colours=8),
|
|
Led('led 3', num_colours=8),
|
|
],
|
|
asset_tag='LED',
|
|
),
|
|
Arduino(
|
|
pins=[
|
|
EmptyPin(), # pin 0
|
|
EmptyPin(), # pin 1
|
|
EmptyPin(), # ultrasonic trigger pin, pin 2
|
|
UltrasonicSensor('ultrasound front'), # pin 3
|
|
EmptyPin(), # ultrasonic trigger pin, pin 4
|
|
UltrasonicSensor('ultrasound left'), # pin 5
|
|
EmptyPin(), # ultrasonic trigger pin, pin 6
|
|
UltrasonicSensor('ultrasound right'), # pin 7
|
|
EmptyPin(), # ultrasonic trigger pin, pin 8
|
|
UltrasonicSensor('ultrasound back'), # pin 9
|
|
MicroSwitch('front left bump sensor'), # pin 10
|
|
MicroSwitch('front right bump sensor'), # pin 11
|
|
MicroSwitch('rear left bump sensor'), # pin 12
|
|
MicroSwitch('rear right bump sensor'), # pin 13
|
|
ReflectanceSensor('left reflectance sensor'), # pin A0
|
|
ReflectanceSensor('center reflectance sensor'), # pin A1
|
|
ReflectanceSensor('right reflectance sensor'), # pin A2
|
|
EmptyPin(), # pin A3
|
|
EmptyPin(), # pin A4
|
|
EmptyPin(), # pin A5
|
|
],
|
|
asset_tag='Arduino1',
|
|
),
|
|
TimeServer(
|
|
asset_tag='TimeServer',
|
|
),
|
|
CameraBoard(
|
|
Camera('camera', frame_rate=15),
|
|
asset_tag='Camera',
|
|
),
|
|
]
|
|
|
|
device_servers: list[DeviceServer] = []
|
|
|
|
for device in devices:
|
|
# connect each device to a socket to receive commands from sr-robot3
|
|
device_servers.append(DeviceServer(device))
|
|
|
|
# collect all device servers into a single server which will handle all connections
|
|
# and commands
|
|
return SocketServer(device_servers)
|
|
|
|
|
|
def main() -> None:
|
|
"""
|
|
Main function to setup and run the devices. Only used for testing.
|
|
|
|
This function will setup the devices and start the select loop to handle all connections.
|
|
"""
|
|
server = setup_devices(logging.DEBUG)
|
|
# generate and print the socket url and information for each device
|
|
print(server.links_formatted())
|
|
# start select loop for all server sockets and device sockets
|
|
server.run()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
logging.basicConfig(level=logging.INFO)
|
|
main()
|