diff --git a/al5d_control.ipynb b/al5d_control.ipynb
index 0c3f58480f7f3c79a7b9c5c3f2c81697843b4264..383e7ffc6e0f9b20559ca2152a5a136c00441ed0 100644
--- a/al5d_control.ipynb
+++ b/al5d_control.ipynb
@@ -19,54 +19,11 @@
"metadata": {},
"outputs": [],
"source": [
- "# Importing necessary modules\n",
- "import serial\n",
- "import sys\n",
- "import time\n",
- "from ipywidgets import interact, interactive, fixed, interact_manual\n",
- "import roboticstoolbox as rtb\n",
- "import numpy as np\n",
+ "from al5d_control import *\n",
"\n",
- "al5d = rtb.models.URDF.AL5D_mdw()\n",
- "\n",
- "def robot_control(q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints\n",
- " q0r = q0*np.pi/180\n",
- " q1r = q1*np.pi/180\n",
- " q2r = q2*np.pi/180\n",
- " q3r = q3*np.pi/180\n",
- " q4r = q4*np.pi/180\n",
- " elbow = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')\n",
- " wrist = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')\n",
- " grip = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')\n",
- " tool = al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')\n",
- " \n",
- " if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):\n",
- " print(\"Robot collision with the table\")\n",
- " else:\n",
- " print(\"No collision detected\")\n",
- " pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)\n",
- " pwq1 = str(round((q1+90)*11.1111 + 500))\n",
- " pwq2 = str(round((q2+90)*11.1111 + 500))\n",
- " pwq3 = str(round((q3+90)*11.1111 + 500))\n",
- " pwq4 = str(round((q4+90)*11.1111 + 500))\n",
- " pwq5 = str(round((gripper+90)*11.1111 + 500))\n",
- " # Concatinating the desired control signals in a long string\n",
- " output = \"#0P\" + pwq0 + \"S300\"+\"#1P\" + pwq1 + \"S300\"+\"#2P\" + pwq2 + \"S300\"+\"#3P\" + pwq3 + \"S1500\" + \"#4P\" + pwq4 + \"S1500\"+ \"#5P\" + pwq5 + \"S1500\"+\"\\r\" # Formatting string\n",
- " output = output.encode('utf-8') # Converting to bytes literals\n",
- " ssc32.write(output) # sending serial data to the SSC-32 board\n",
- "\n",
- "devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']\n",
- " \n",
- "# Starting the connection with the SSC-32U controller\n",
- "for device in devices:\n",
- " try:\n",
- " ssc32 = serial.Serial(device, 9600, timeout=1.0)\n",
- " except serial.SerialException:\n",
- " print(\"No device connected on \", device)\n",
- "# Initializing the position of the robot\n",
- "ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\\r')\n",
- "# Initialising the sliders\n",
- "interact(robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))"
+ "rrob = AL5DControl()\n",
+ "rrob.send_commands()\n",
+ "\n"
]
}
],
diff --git a/al5d_control.py b/al5d_control.py
new file mode 100644
index 0000000000000000000000000000000000000000..5b27367306d39ecc6671c03729b33148746f960e
--- /dev/null
+++ b/al5d_control.py
@@ -0,0 +1,56 @@
+# Importing necessary modules
+import serial
+import sys
+import time
+from ipywidgets import interact, interactive, fixed, interact_manual
+import roboticstoolbox as rtb
+import numpy as np
+
+class AL5DControl:
+ def __init__(self):
+
+ # al5d model
+ self.al5d = rtb.models.URDF.AL5D_mdw()
+
+ # search for port, initialise in home pose
+ devices = ['/dev/ttyUSB0','/dev/ttyUSB1','COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9']
+
+ # Starting the connection with the SSC-32U controller
+ for device in devices:
+ try:
+ self.ssc32 = serial.Serial(device, 9600, timeout=1.0)
+ # Initializing the position of the robot
+ self.ssc32.write(b'#0P1500S100#1P1150S200#2P1500S300#3P1500S350#4P1500S100#5P1500S150\r')
+ except serial.SerialException:
+ print("No device connected on ", device)
+
+ def robot_control(self, q0=0, q1=0, q2=0, q3=0, q4=0, gripper=0): #Method for controlling the joints
+ q0r = q0*np.pi/180
+ q1r = q1*np.pi/180
+ q2r = q2*np.pi/180
+ q3r = q3*np.pi/180
+ q4r = q4*np.pi/180
+ elbow = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='elbow')
+ wrist = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='wrist')
+ grip = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='gripper')
+ tool = self.al5d.fkine([q0r,q1r,q2r,q3r,q4r], end='tool')
+
+ if any(np.array([tool.t[2], grip.t[2], elbow.t[2], wrist.t[2]]) < 0.06):
+ print("Robot collision with the table")
+ else:
+ print("No collision detected")
+ pwq0 = str(round((q0+90)*11.1111 + 500)) # Scaling of the joint signal from (0,180) to (500,2500)
+ pwq1 = str(round((q1+90)*11.1111 + 500))
+ pwq2 = str(round((q2+90)*11.1111 + 500))
+ pwq3 = str(round((q3+90)*11.1111 + 500))
+ pwq4 = str(round((q4+90)*11.1111 + 500))
+ pwq5 = str(round((gripper+90)*11.1111 + 500))
+ # Concatinating the desired control signals in a long string
+ output = "#0P" + pwq0 + "S300"+"#1P" + pwq1 + "S300"+"#2P" + pwq2 + "S300"+"#3P" + pwq3 + "S1500" + "#4P" + pwq4 + "S1500"+ "#5P" + pwq5 + "S1500"+"\r" # Formatting string
+ output = output.encode('utf-8') # Converting to bytes literals
+ self.ssc32.write(output) # sending serial data to the SSC-32 board
+
+ def send_commands(self):
+ # Initialising the sliders
+ interact(self.robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90))
+