Merge pull request #32 from alberto-abarzua/31-add-tool-settings-support-and-improve-flashing

31 add tool settings support and improve flashing

Author: alberto-abarzua

.env.template

@@ -3,6 +3,7 @@ export ESP_WIFI_SSID=ssid
 export ESP_WIFI_PASSWORD=password
 export ESP_CONTROLLER_SERVER_HOST=backend
 export ESP_CONTROLLER_SERVER_PORT=8500
+export ESP_FLASH_PORT=
 
 # CONTROLLER DEFAULT SETTINGS
 CONTROLLER_WEBSOCKET_PORT=8600

backend/src/config/main_arm.toml

@@ -35,9 +35,17 @@ title = "Robot arm configuration"
     a6x= 169.0
     a6z= 0
     a6y= 0
+[tool]
+    name = "tool"
+    max_angle_rad = 0.0
+    min_angle_rad = 0.0
+    [tool.driver]
+        type = "servo"
+        pin = 26
+    
 
 [[joints]]
-    name = "joinT_1"
+    name = "joint_1"
     homing_direction = -1 # 1 or -1
     conversion_rate_axis_joint = 9.3 # Gear ratio
     homing_offset_rad = 0 # Offset from home position
@@ -51,7 +59,7 @@ title = "Robot arm configuration"
     #     type = "servo"
     #     pin = 20
     [joints.endstop]
-        type = "hall"
+        type = "none"
         pin = 26
     # [joints.endstop]
     #     type = "dummy"
@@ -82,7 +90,7 @@ title = "Robot arm configuration"
         dir_pin = 16
         step_pin = 15
     [joints.endstop]
-        type = "hall"
+        type = "none"
         pin = 33
 
 [[joints]]
@@ -108,7 +116,7 @@ title = "Robot arm configuration"
         dir_pin = 21
         step_pin = 19
     [joints.endstop]
-        type = "hall"
+        type = "none"
         pin = 35
 
 [[joints]]
@@ -121,6 +129,6 @@ title = "Robot arm configuration"
         dir_pin = 23
         step_pin = 22 
     [joints.endstop]
-        type = "hall"
+        type = "none"
         pin = 34
 

backend/src/main.py

@@ -1,11 +1,23 @@
+from contextlib import asynccontextmanager
+
 from fastapi import FastAPI
 from fastapi.middleware.cors import CORSMiddleware
 
 from routers.move import router as move_router
 from routers.settings import router as settings_router
 from utils.general import start_controller, stop_controller
 
-app = FastAPI()
+
+@asynccontextmanager
+async def controller_lifespan(_: FastAPI):  # type: ignore # noqa: ANN201
+    start_controller()
+
+    yield
+
+    stop_controller()
+
+
+app = FastAPI(lifespan=controller_lifespan)
 
 app.add_middleware(
     CORSMiddleware,
@@ -18,13 +30,3 @@
 
 app.include_router(move_router, prefix="/move")
 app.include_router(settings_router, prefix="/settings")
-
-
-@app.on_event("startup")
-async def startup_event() -> None:
-    start_controller()
-
-
-@app.on_event("shutdown")
-async def shutdown_event() -> None:
-    stop_controller()

backend/src/routers/move.py

@@ -5,6 +5,7 @@
 from pydantic import BaseModel
 from robot_arm_controller.control.arm_kinematics import ArmPose
 from robot_arm_controller.controller import ArmController
+from robot_arm_controller.utils.algebra import degree2rad
 
 from utils.general import controller_dependency
 
@@ -23,17 +24,20 @@ class Move(BaseModel):
     pitch: float
     yaw: float
     wait: Optional[bool] = False
+    degrees: Optional[bool] = True
 
 
 class Tool(BaseModel):
     toolValue: float
     wait: Optional[bool] = False
+    degrees: Optional[bool] = True
 
 
 class MoveJoint(BaseModel):
     joint_idx: int
     joint_value: float
     wait: Optional[bool] = False
+    degrees: Optional[bool] = True
 
 
 class HomeJoint(BaseModel):
@@ -44,6 +48,7 @@ class HomeJoint(BaseModel):
 class MoveJoints(BaseModel):
     joint_values: list
     wait: Optional[bool] = False
+    degrees: Optional[bool] = True
 
 
 # --------
@@ -76,8 +81,9 @@ def home_joint(
 def move(move: Move, controller: ArmController = controller_dependency) -> JSONResponse:
     move_dict = move.model_dump()
     wait = move_dict.pop("wait")
+    degrees = move_dict.pop("degrees")
 
-    pose = ArmPose(**move_dict)
+    pose = ArmPose(**move_dict, degree=degrees)
 
     move_is_possible = controller.move_to(pose)
 
@@ -95,8 +101,9 @@ def move_relative(
 ) -> JSONResponse:
     move_dict = move.model_dump()
     wait = move_dict.pop("wait")
+    degrees = move_dict.pop("degrees")
 
-    pose = ArmPose(**move_dict)
+    pose = ArmPose(**move_dict, degree=degrees)
 
     move_is_possible = controller.move_to_relative(pose)
 
@@ -114,7 +121,9 @@ def valid_pose(
 ) -> JSONResponse:
     move_dict = move.model_dump()
     move_dict.pop("wait")
-    pose = ArmPose(**move_dict)
+    degrees = move_dict.pop("degrees")
+    pose = ArmPose(**move_dict, degree=degrees)
+
     move_is_possible = controller.valid_pose(pose)
     if move_is_possible:
         return JSONResponse(content={"message": "Pose is valid"}, status_code=200)
@@ -133,9 +142,14 @@ def move_joint(
 ) -> Dict[Any, Any]:
     move_dict = move.model_dump()
     wait = move_dict.pop("wait")
+    degrees = move_dict.pop("degrees")
 
     joint_idx = move_dict.pop("joint_idx")
     joint_value = move_dict.pop("joint_value")
+
+    if degrees:
+        joint_value = degree2rad(joint_value)
+
     controller.move_joint_to(joint_idx, joint_value)
     if wait:
         controller.wait_done_moving()
@@ -148,9 +162,13 @@ def move_joint_to_relative(
 ) -> Dict[Any, Any]:
     move_dict = move.model_dump()
     wait = move_dict.pop("wait")
+    degrees = move_dict.pop("degrees")
 
     joint_idx = move_dict.pop("joint_idx")
     joint_value = move_dict.pop("joint_value")
+    if degrees:
+        joint_value = degree2rad(joint_value)
+
     controller.move_joint_to_relative(joint_idx, joint_value)
     if wait:
         controller.wait_done_moving()
@@ -163,8 +181,12 @@ def move_joints_to_relative(
 ) -> Dict[Any, Any]:
     move_dict = move.model_dump()
     wait = move_dict.pop("wait")
+    degrees = move_dict.pop("degrees")
 
     joint_values = move_dict.pop("joint_values")
+    if degrees:
+        joint_values = [degree2rad(joint_value) for joint_value in joint_values]
+
     controller.move_joints_to_relative(joint_values)
     if wait:
         controller.wait_done_moving()
@@ -176,21 +198,42 @@ def move_joints_to_relative(
 # --------
 
 
-@router.post("/tool/move/")
+@router.post("/tool/")
 def tool_post(
     tool: Tool, controller: ArmController = controller_dependency
 ) -> Dict[Any, Any]:
     tool_dict = tool.model_dump()
     tool_value = tool_dict["toolValue"]
     wait = tool_dict["wait"]
+    degrees = tool_dict["degrees"]
+    if degrees:
+        tool_value = degree2rad(tool_value)
+
     controller.set_tool_value(tool_value)
     if wait:
         controller.wait_done_moving()
     return {"message": "Moved"}
 
 
+@router.post("/tool/relative/")
+def tool_relative(
+    tool: Tool, controller: ArmController = controller_dependency
+) -> Dict[Any, Any]:
+    tool_dict = tool.model_dump()
+    tool_value = tool_dict["toolValue"]
+    wait = tool_dict["wait"]
+    degrees = tool_dict["degrees"]
+    if degrees:
+        tool_value = degree2rad(tool_value)
+
+    print("\n\n\ntool_value", tool_value)
+    controller.set_tool_value_relative(tool_value)
+    if wait:
+        controller.wait_done_moving()
+    return {"message": "Moved"}
+
+
 @router.get("/tool/current/")
 def tool_get(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
     tool_value = controller.tool_value
-
     return {"toolValue": tool_value}

backend/src/routers/settings.py

@@ -2,9 +2,10 @@
 from pathlib import Path
 from typing import Any, Dict, Optional
 
-from fastapi import APIRouter
+from fastapi import APIRouter, Query
 from pydantic import BaseModel
 from robot_arm_controller.controller import ArmController, ControllerStatus, Settings
+from robot_arm_controller.utils.algebra import rad2degree
 
 from utils.general import controller_dependency
 
@@ -41,13 +42,27 @@ def get_items(
 
 
 @router.get("/status/")
-def status(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
+def status(
+    controller: ArmController = controller_dependency,
+    degrees: bool = Query(True, description="Return angles in degrees"),
+) -> Dict[Any, Any]:
     pose = controller.current_pose
-    status_dict: Dict[str, Any] = deepcopy(pose.as_dict)
+    status_dict: Dict[str, Any] = deepcopy(pose.get_dict(degrees=degrees))
+
     status_dict["toolValue"] = controller.tool_value
+    if degrees:
+        status_dict["toolValue"] = rad2degree(status_dict["toolValue"])
+
     status_dict["isHomed"] = controller.is_homed
     status_dict["moveQueueSize"] = controller.move_queue_size
-    status_dict["currentAngles"] = controller.current_angles
+    controller_angles = controller.current_angles
+    if degrees:
+        currentAngles = []
+        for angle in controller_angles:
+            currentAngles.append(rad2degree(angle))
+        status_dict["currentAngles"] = currentAngles
+    else:
+        status_dict["currentAngles"] = controller_angles
 
     status_dict["connected"] = controller.status == ControllerStatus.RUNNING
     status_dict["status"] = controller.status

controller/src/robot_arm_controller/control/arm_kinematics.py

@@ -149,6 +149,26 @@ def as_dict(self) -> Dict[str, float]:
             "yaw": self.yaw,
         }
 
+    def get_dict(self, degrees: bool = False) -> Dict[str, float]:
+        if not degrees:
+            return {
+                "x": self.x,
+                "y": self.y,
+                "z": self.z,
+                "roll": self.roll,
+                "pitch": self.pitch,
+                "yaw": self.yaw,
+            }
+        else:
+            return {
+                "x": self.x,
+                "y": self.y,
+                "z": self.z,
+                "roll": np.rad2deg(self.roll),
+                "pitch": np.rad2deg(self.pitch),
+                "yaw": np.rad2deg(self.yaw),
+            }
+
     def __add__(self, other: "ArmPose") -> "ArmPose":
         if not isinstance(other, ArmPose):
             raise ValueError("Can only add two ArmPose instances")

controller/src/robot_arm_controller/controller.py

@@ -196,6 +196,14 @@ def configure_from_file(self, file: Path, reload: bool = True) -> None:
         for key, value in arm_params.items():
             self.arm_params.__setattr__(key, value)
 
+        # Tool settings
+
+        tool = contents["tool"]
+        tool_driver = tool["driver"]
+        if tool_driver["type"] == "servo":
+            pin = int(tool_driver["pin"])
+            self.set_tool_driver_servo(pin)
+
         default_speed = joint_configuration["speed_rad_per_s"]
         default_homing_direction = joint_configuration["homing_direction"]
         default_steps_per_rev_motor_axis = joint_configuration["steps_per_rev_motor_axis"]
@@ -263,8 +271,10 @@ def configure_from_file(self, file: Path, reload: bool = True) -> None:
             self.set_setting_joint(Settings.CONVERSION_RATE_AXIS_JOINTS, conversion_rate_axis_joint, i)
             self.set_setting_joint(Settings.HOMING_OFFSET_RADS, homing_offset_rads, i)
             self.set_setting_joint(Settings.DIR_INVERTED, dir_inverted, i)
+
             self.arm_params.joints[i].set_bounds(min_angle_rad, max_angle_rad)
 
+        self.print_state()
         if reload:
             self.file_reload_thread = threading.Thread(target=self.check_and_reload_config, args=(file,))
             self.file_reload_thread.start()
@@ -416,6 +426,10 @@ def set_tool_value(self, angle: float) -> None:
         if self.print_status:
             console.log(f"Setting tool value to: {angle}", style="set_tool")
 
+    def set_tool_value_relative(self, angle: float) -> None:
+        target_angle = self.tool_value + angle
+        self.set_tool_value(target_angle)
+
     def wait_until_angles_at_target(self, target_angles: List[float], epsilon: float = 0.01) -> None:
         while not allclose(self.current_angles, target_angles, atol=epsilon) and not self.stop_event.is_set():
             time.sleep(0.2)
@@ -446,6 +460,12 @@ def stop_movement(self) -> None:
         if self.print_status:
             console.log("Stopping arm", style="info")
 
+    def print_state(self) -> None:
+        message = Message(MessageOp.STATUS, 7)
+        self.controller_server.send_message(message, mutex=True)
+        if self.print_status:
+            console.log("Printing arm state", style="info")
+
     """
     ----------------------------------------
                     API Methods -- CONFIG
@@ -515,6 +535,12 @@ def set_joint_endstop_none(self, joint_idx: int) -> None:
         if self.print_status:
             console.log(f"Setting endstop none for joint {joint_idx}", style="set_settings")
 
+    def set_tool_driver_servo(self, pin: int) -> None:
+        message = Message(MessageOp.CONFIG, 35, [float(pin)])
+        self.controller_server.send_message(message, mutex=True)
+        if self.print_status:
+            console.log(f"Setting servo driver for tool on pin {pin}", style="set_settings")
+
 
 class SingletonArmController:
     _instance: Optional[ArmController] = None