Moving robot code to robot v1

src/api/v1/api.py

@@ -12,7 +12,7 @@
 import cv2
 
 app = FastAPI()
-camera = cv2.VideoCapture(1, cv2.CAP_DSHOW)
+camera = cv2.VideoCapture("/dev/video2")
 templates = Jinja2Templates(directory="templates")
 
 origins = [

src/api/v1/robot.py

@@ -120,6 +120,9 @@ def __init__(
         self.initialized = False
 
     def __validate_command(self):
+        """
+        Run validation against the motor command
+        """
         if not self.initialized:
             raise Exception("Motor needs to be initialized before commanding")
 
@@ -182,10 +185,15 @@ def __read_bytes(self, offset: int, size: int) -> Optional[int]:
     def __get_memory_property(self, property: str) -> int:
         self.__validate_command()
 
-        value = self.__read_bytes(
-            getattr(self.address_config, property).byte_offset,
-            getattr(self.address_config, property).byte_size,
-        )
+        try:
+            config: MemorySegment = getattr(self.address_config, property)
+        except:
+            # Swallow the missing attribute error and throw a NotImplementedError for the motor instead
+            raise NotImplementedError(
+                f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!"
+            )
+
+        value = self.__read_bytes(config.byte_offset, config.byte_size)
 
         if value is None:
             raise Exception(f"FATAL | DXID: '{self.id}' | Value could not be read!")
@@ -195,9 +203,17 @@ def __get_memory_property(self, property: str) -> int:
     def __set_memory_property(self, property: str, value: int):
         self.__validate_command()
 
+        try:
+            config: MemorySegment = getattr(self.address_config, property)
+        except:
+            # Swallow the missing attribute error and throw a NotImplementedError for the motor instead
+            raise NotImplementedError(
+                f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!"
+            )
+
         self.__write_bytes(
-            getattr(self.address_config, property).byte_offset,
-            getattr(self.address_config, property).byte_size,
+            config.byte_offset,
+            config.byte_size,
             value,
         )
 

src/api/v2/api.py

@@ -1,27 +1,41 @@
-from typing import Union
 from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect
 from websockets.exceptions import ConnectionClosed
 from fastapi.templating import Jinja2Templates
+from fastapi.middleware.cors import CORSMiddleware
+from robot import (
+    Position2D,
+    RobotControl,
+    DynamixelMotor,
+    DYNAMIXEL_MX_12_ADDR_CONFIG,
+)
 import asyncio
 import cv2
 
 app = FastAPI()
-camera = cv2.VideoCapture(0, cv2.CAP_DSHOW)
+camera = cv2.VideoCapture("/dev/video2")
 templates = Jinja2Templates(directory="templates")
 
+origins = [
+    "http://localhost:3000",
+]
 
-@app.get("/")
-def read_root():
-    return {"Hello": "World"}
+app.add_middleware(
+    CORSMiddleware,
+    allow_origins=origins,
+    allow_credentials=True,
+    allow_methods=["*"],  # Allow all HTTP methods
+    allow_headers=["*"],  # Allow all headers
+)
 
-
-@app.get("/items/{item_id}")
-def read_item(item_id: int, q: Union[str, None] = None):
-    return {"item_id": item_id, "q": q}
+motors = [
+    DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)),
+    DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True),
+]
+ctrl = RobotControl("/dev/ttyUSB0", 1, motors)
+ctrl.init(1000000)
 
 
 # https://stackoverflow.com/a/70626324
-# Not actually sure if this works with bazel
 @app.websocket("/ws")
 async def get_stream(websocket: WebSocket):
     await websocket.accept()
@@ -36,3 +50,15 @@ async def get_stream(websocket: WebSocket):
             await asyncio.sleep(0.03)
     except (WebSocketDisconnect, ConnectionClosed):
         print("Client disconnected")
+
+
+@app.post("/command")
+async def command(request: Request):
+    data = await request.json()
+
+    x_val = -data.get("left", 0) + data.get("right", 0)
+    y_val = -data.get("down", 0) + data.get("up", 0)
+
+    ctrl.set_velocity(Position2D(x_val, y_val, 0))
+
+    return {"status": "success"}

src/robot/v1/BUILD.bazel

@@ -0,0 +1,17 @@
+load("@rules_python//python:defs.bzl", "py_library")
+
+# Put all robot files in this library
+py_library(
+    name = "robot_src",
+    srcs = [
+        "config.py",
+        "robot.py",
+        "structs.py",
+    ],
+    deps = [
+        "@pypi//fastapi:pkg",
+        "@pypi//jinja2:pkg",
+        "@pypi//opencv_python:pkg",
+        "@pypi//websockets:pkg",
+    ],
+)

src/robot/v1/config.py

@@ -0,0 +1,24 @@
+from structs import DynamixelMotorAddressConfig, MemorySegment
+
+# Add all Dynamixel Motor Configs here
+DYNAMIXEL_MX_12_ADDR_CONFIG = DynamixelMotorAddressConfig(
+    torque_enable=MemorySegment(24, 1),
+    led_enable=MemorySegment(25, 1),
+    d_gain=MemorySegment(26, 1),
+    i_gain=MemorySegment(27, 1),
+    p_gain=MemorySegment(28, 1),
+    goal_position=MemorySegment(30, 2),
+    moving_speed=MemorySegment(32, 2),
+    torque_limit=MemorySegment(34, 2),
+    present_position=MemorySegment(36, 2),
+    present_speed=MemorySegment(38, 2),
+    present_load=MemorySegment(40, 2),
+    present_input_voltage=MemorySegment(42, 1),
+    present_temperature=MemorySegment(43, 1),
+    registered=MemorySegment(44, 1),
+    moving=MemorySegment(46, 1),
+    lock=MemorySegment(47, 1),
+    punch=MemorySegment(48, 2),
+    realtime_tick=MemorySegment(50, 2),
+    goal_acceleration=MemorySegment(73, 1),
+)