matlab source code

src/matlab/ABB_SCARA_IRB.m

@@ -0,0 +1,46 @@
+clc;
+clear all;
+
+a1 = 0.2;
+a2 = 0.25;
+
+dh = [
+    0   0   a1  0   0;
+    0   0   a2  pi  0;
+    0   0   0   0   1;
+    0   0   0   0   0];
+
+R = UnityLink(dh, 'name', 'ABB SCARA IRB');
+
+R.qlim = [
+    -deg2rad(140)       deg2rad(140);
+    -deg2rad(150)       deg2rad(150);
+    0                   0.18;
+    -deg2rad(400)       deg2rad(400)];
+
+dt = 0.01;
+t1 = abs(R.qlim(1, 2) - R.qlim(1, 1));
+t2 = abs(R.qlim(2, 2) - R.qlim(2, 1));
+t3 = abs(R.qlim(3, 2) - R.qlim(3, 1));
+t4 = abs(R.qlim(4, 2) - R.qlim(4, 1));
+
+q1 = R.qlim(1, 1):0.01:R.qlim(1, 2);
+q2 = R.qlim(2, 1):0.01:R.qlim(2, 2);
+q3 = R.qlim(3, 1):0.01:R.qlim(3, 2);
+q4 = R.qlim(4, 1):0.01:R.qlim(4, 2);
+
+disp([length(q1) length(q2) length(q3) length(q4)])
+
+pause();
+for i=1:length(q1)
+    %R.plot([q1(i) 0 0 0]);
+    R.sendQ([q1(i) 0 0 0]);
+end
+pause();
+for i=1:length(q2)
+    %R.plot([q1(i) 0 0 0]);
+    R.sendQ([0 q2(i) 0 0]);
+end
+
+R.plot([0 0 0 0]);
+%R.teach;

src/matlab/UnityLink.m

@@ -0,0 +1,74 @@
+classdef UnityLink < SerialLink
+    properties
+        ip
+        port
+        timeout
+    end
+
+    properties (Access = private)
+        tcpipClient
+    end
+
+    methods
+        function r = UnityLink(varargin)
+        %SerialLink Creat a UnityLink robot object
+
+            % default properties
+            default.ip = '172.23.48.46';
+            default.port = 8888;
+            default.timeout = 30;
+
+            % process the rest of the arguments in key, value pairs
+            opt.ip = [];
+            opt.port = [];
+            opt.timeout = [];
+
+            [opt, arg] = tb_optparse(opt, varargin);
+
+            r = r@SerialLink(arg{:});
+
+            % set properties of robot object from passed options or
+            % defaults
+            for p = properties(r)'
+                p = p{1};   % property name
+
+                if isfield(opt, p) && ~isempty( opt.(p) )
+                    % if there's a set option, override what's in the robot
+                    r.(p) = opt.(p);
+                end
+
+                if isfield(default, p) && isempty( r.(p) )
+                    % otherwise if there's a set default, use that
+                    r.(p) = default.(p); 
+                end
+            end
+
+            % TCP/IP communication config
+            r.tcpipClient = tcpip(r.ip, r.port, 'NetworkRole', 'Client');
+            set(r.tcpipClient, 'Timeout', r.timeout);
+        end
+
+        function plot(r, q)
+            plot@SerialLink(r, q);
+            r.sendQ(q);
+        end
+
+        function sendQ(r, q)
+            msg = ":";
+            for i=1:length(q)
+                msg = msg + num2str(q(i)) + ':';
+            end
+            r.sendMsg(msg);
+        end
+
+    end
+
+    methods (Access = private)
+        function sendMsg(r, msg)
+        %UnityLink.sendMsg Send message to Unity app
+            fopen(r.tcpipClient);
+            fwrite(r.tcpipClient, msg);
+            fclose(r.tcpipClient);
+        end
+    end
+end