{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "71c07e92-14e6-4a13-b7ef-182b15636f72",
   "metadata": {},
   "source": [
    "# Controlling Dobot"
   ]
  },
  {
   "cell_type": "code",
   "id": "9533a8ff-f102-44f4-a7cc-0274cd1627f5",
   "metadata": {},
   "source": [
    "from pathlib import Path\n",
    "from dobot1_control import DobotControl\n",
    "import numpy as np\n",
    "\n",
    "# fix permissions possibly:\n",
    "#   sudo chmod o+rw /dev/ttyACM0\n",
    "# or for many places:\n",
    "#   for dev in /dev/ttyACM*; do echo $dev; sudo chmod o+rw \"$dev\"; done\n",
    "comport = str(next(Path(\"/dev\").glob(\"ttyACM*\")))\n",
    "dobot = DobotControl(comport, endEffectorOffset=(49., 64.), accelOffset=(1016.39, 1009.08), accelConversion=509.00, plot=True, debug=True)\n",
    "# percentage of maximum joint speed\n",
    "speed = 0.5\n",
    "# percentage of maximum joint acceleration\n",
    "acceleration = 0.5\n",
    "curpos = dobot.pos"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": "dobot.pos",
   "id": "47a9a53e98c0cb38",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "# this should be a no-op that should not fail\n",
    "dobot.MoveWithSpeed((curpos+(0,0,0),), speed, acceleration)"
   ],
   "id": "972968593670fe28",
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "id": "b3d66853-b372-4cc7-ad46-0d8940b13e05",
   "metadata": {},
   "source": [
    "# do a rectangular motion for testing the motion planning which must stop at the edge\n",
    "#nextPos += (10,0,0)  # adjust for relative movement\n",
    "dobot.MoveWithSpeed((curpos, curpos + (-2, 0, 0), curpos + (20, 0, 0), curpos + (20, 20, 0), curpos + (20, 22, 0), curpos), speed, acceleration)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "pts = [\n",
    "    np.array([230.94713508,   0.        ,  94.01584856]),\n",
    "    np.array([228.98939573,  12.36067977,  94.01584856]),\n",
    "    np.array([223.30781486,  23.51141009,  94.01584856]),\n",
    "    np.array([214.45854517,  32.36067977,  94.01584856]),\n",
    "    np.array([203.30781486,  38.04226065,  94.01584856]),\n",
    "    np.array([190.94713508,  40.        ,  94.01584856]),\n",
    "    np.array([178.58645531,  38.04226065,  94.01584856]),\n",
    "    np.array([167.43572499,  32.36067977,  94.01584856]),\n",
    "    np.array([158.58645531,  23.51141009,  94.01584856]),\n",
    "    np.array([152.90487443,  12.36067977,  94.01584856]),\n",
    "    np.array([1.50947135e+02, 4.89858720e-15, 9.40158486e+01]),\n",
    "    np.array([152.90487443, -12.36067977,  94.01584856]),\n",
    "]\n",
    "dobot.MoveWithSpeed(pts, speed, acceleration)"
   ],
   "id": "9771cde8b3b8be4",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": "dobot.MoveWithSpeed((curpos + (40, 40, 0), curpos + (40, 45, 0), curpos + (40, -40, 0)), speed, acceleration)",
   "id": "381816da54856d9a",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "def circle_points(center, radius, n=100):\n",
    "    cx, cy = center[:2]\n",
    "    theta = np.linspace(0, 2*np.pi, n, endpoint=False)\n",
    "    x = cx + radius * np.cos(theta)\n",
    "    y = cy + radius * np.sin(theta)\n",
    "    pts = np.column_stack((x, y))\n",
    "    # combine and repeat the first point at the end\n",
    "    return np.vstack((pts, pts[0]))\n",
    "\n",
    "def plot_points(x, y, center):\n",
    "    plt.figure(figsize=(5,5))\n",
    "    plt.plot(x, y, marker='o', markersize=4, linestyle='-')   # circle\n",
    "    plt.scatter(*center, color='red')                    # center\n",
    "    plt.gca().set_aspect('equal', 'box')                      # equal scaling\n",
    "    plt.grid(True)\n",
    "    plt.xlabel('x')\n",
    "    plt.ylabel('y')\n",
    "    plt.title(f\"Circle center: {tuple(f\"{v:.2f}\" for v in center[0:2])}, {radius=})\")\n",
    "    plt.show()"
   ],
   "id": "8e475ef9db10177a",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "# first point\n",
    "first = (220, 0., 15)\n",
    "dobot.MoveWithSpeed((first,), speed, acceleration)\n",
    "curpos = dobot.pos\n",
    "curpos"
   ],
   "id": "2fc160da0beaba35",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "radius = 40\n",
    "pts = circle_points(curpos, radius, n=20)\n",
    "plot_points(pts[:,0], pts[:,1], curpos)\n",
    "zPos = 11\n",
    "pts3 = [np.array((*pt, zPos)) for pt in pts]\n",
    "pts3"
   ],
   "id": "83445273a6711f36",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "dobot.MoveWithSpeed(pts3, .5*speed, acceleration)\n",
    "# back to start pos (center)\n",
    "dobot.MoveWithSpeed(curpos, .5*speed, acceleration)"
   ],
   "id": "ae76eaf1569b642c",
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "code",
   "source": [
    "for pt in pts3[:3]:\n",
    "    print(f\"Going to {pt} ..\")\n",
    "    dobot.MoveWithSpeed(pt, speed, acceleration)"
   ],
   "id": "d64de9c5cb7c0559",
   "outputs": [],
   "execution_count": null
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.14.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}