├── doc └── img │ ├── pen.png │ ├── prm.png │ ├── dStar.png │ ├── drawPath.png │ ├── lexyRobotGUI.png │ └── lexyRobotHardware.png ├── matlab ├── LexyRobotGUI.fig ├── letters │ ├── letterA.mat │ ├── letterB.mat │ ├── letterC.mat │ ├── letterD.mat │ ├── letterE.mat │ ├── letterF.mat │ ├── letterG.mat │ ├── letterH.mat │ ├── letterI.mat │ ├── letterJ.mat │ ├── letterK.mat │ ├── letterL.mat │ ├── letterM.mat │ ├── letterN.mat │ ├── letterO.mat │ ├── letterP.mat │ ├── letterQ.mat │ ├── letterR.mat │ ├── letterS.mat │ ├── letterT.mat │ ├── letterU.mat │ ├── letterV.mat │ ├── letterW.mat │ ├── letterX.mat │ ├── letterY.mat │ ├── letterZ.mat │ ├── nletterA.mat │ ├── nletterB.mat │ ├── nletterC.mat │ ├── nletterD.mat │ ├── nletterE.mat │ ├── nletterF.mat │ ├── nletterG.mat │ ├── nletterH.mat │ ├── nletterI.mat │ ├── nletterL.mat │ ├── nletterM.mat │ ├── nletterN.mat │ ├── nletterO.mat │ ├── nletterP.mat │ ├── nletterQ.mat │ ├── nletterR.mat │ ├── nletterS.mat │ ├── nletterT.mat │ ├── nletterU.mat │ ├── nletterV.mat │ ├── nletterW.mat │ ├── nletterX.mat │ ├── nletterY.mat │ └── nletterZ.mat ├── pen │ ├── license.txt │ └── pen.m ├── testLexyRobot.m ├── lexyRobot.m └── LexyRobotGUI.m └── README.md /doc/img/pen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/doc/img/pen.png -------------------------------------------------------------------------------- /doc/img/prm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/doc/img/prm.png -------------------------------------------------------------------------------- /doc/img/dStar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/doc/img/dStar.png -------------------------------------------------------------------------------- /doc/img/drawPath.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/doc/img/drawPath.png -------------------------------------------------------------------------------- /doc/img/lexyRobotGUI.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/doc/img/lexyRobotGUI.png -------------------------------------------------------------------------------- /matlab/LexyRobotGUI.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/LexyRobotGUI.fig -------------------------------------------------------------------------------- /matlab/letters/letterA.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterA.mat -------------------------------------------------------------------------------- /matlab/letters/letterB.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterB.mat -------------------------------------------------------------------------------- /matlab/letters/letterC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterC.mat -------------------------------------------------------------------------------- /matlab/letters/letterD.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterD.mat -------------------------------------------------------------------------------- /matlab/letters/letterE.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterE.mat -------------------------------------------------------------------------------- /matlab/letters/letterF.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterF.mat -------------------------------------------------------------------------------- /matlab/letters/letterG.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterG.mat -------------------------------------------------------------------------------- /matlab/letters/letterH.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterH.mat -------------------------------------------------------------------------------- /matlab/letters/letterI.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterI.mat -------------------------------------------------------------------------------- /matlab/letters/letterJ.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterJ.mat -------------------------------------------------------------------------------- /matlab/letters/letterK.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterK.mat -------------------------------------------------------------------------------- /matlab/letters/letterL.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterL.mat -------------------------------------------------------------------------------- /matlab/letters/letterM.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterM.mat -------------------------------------------------------------------------------- /matlab/letters/letterN.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterN.mat -------------------------------------------------------------------------------- /matlab/letters/letterO.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterO.mat -------------------------------------------------------------------------------- /matlab/letters/letterP.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterP.mat -------------------------------------------------------------------------------- /matlab/letters/letterQ.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterQ.mat -------------------------------------------------------------------------------- /matlab/letters/letterR.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterR.mat -------------------------------------------------------------------------------- /matlab/letters/letterS.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterS.mat -------------------------------------------------------------------------------- /matlab/letters/letterT.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterT.mat -------------------------------------------------------------------------------- /matlab/letters/letterU.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterU.mat -------------------------------------------------------------------------------- /matlab/letters/letterV.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterV.mat -------------------------------------------------------------------------------- /matlab/letters/letterW.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterW.mat -------------------------------------------------------------------------------- /matlab/letters/letterX.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterX.mat -------------------------------------------------------------------------------- /matlab/letters/letterY.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterY.mat -------------------------------------------------------------------------------- /matlab/letters/letterZ.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/letterZ.mat -------------------------------------------------------------------------------- /matlab/letters/nletterA.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterA.mat -------------------------------------------------------------------------------- /matlab/letters/nletterB.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterB.mat -------------------------------------------------------------------------------- /matlab/letters/nletterC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterC.mat -------------------------------------------------------------------------------- /matlab/letters/nletterD.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterD.mat -------------------------------------------------------------------------------- /matlab/letters/nletterE.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterE.mat -------------------------------------------------------------------------------- /matlab/letters/nletterF.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterF.mat -------------------------------------------------------------------------------- /matlab/letters/nletterG.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterG.mat -------------------------------------------------------------------------------- /matlab/letters/nletterH.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterH.mat -------------------------------------------------------------------------------- /matlab/letters/nletterI.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterI.mat -------------------------------------------------------------------------------- /matlab/letters/nletterL.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterL.mat -------------------------------------------------------------------------------- /matlab/letters/nletterM.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterM.mat -------------------------------------------------------------------------------- /matlab/letters/nletterN.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterN.mat -------------------------------------------------------------------------------- /matlab/letters/nletterO.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterO.mat -------------------------------------------------------------------------------- /matlab/letters/nletterP.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterP.mat -------------------------------------------------------------------------------- /matlab/letters/nletterQ.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterQ.mat -------------------------------------------------------------------------------- /matlab/letters/nletterR.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterR.mat -------------------------------------------------------------------------------- /matlab/letters/nletterS.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterS.mat -------------------------------------------------------------------------------- /matlab/letters/nletterT.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterT.mat -------------------------------------------------------------------------------- /matlab/letters/nletterU.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterU.mat -------------------------------------------------------------------------------- /matlab/letters/nletterV.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterV.mat -------------------------------------------------------------------------------- /matlab/letters/nletterW.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterW.mat -------------------------------------------------------------------------------- /matlab/letters/nletterX.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterX.mat -------------------------------------------------------------------------------- /matlab/letters/nletterY.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterY.mat -------------------------------------------------------------------------------- /matlab/letters/nletterZ.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/matlab/letters/nletterZ.mat -------------------------------------------------------------------------------- /doc/img/lexyRobotHardware.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akafael/LexyRobot/HEAD/doc/img/lexyRobotHardware.png -------------------------------------------------------------------------------- /matlab/pen/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009, Jordi Palacin 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | LexyRobot 2 | ========= 3 | 4 | Arduino Robotic Arm with 2 degrees of freedom controled by MATLAB. Project deverloped with Chris Zachariah as part of the course ECE 5995 Introduction to Robotics at Wayne State from [Dr Abhilash Pandya](http://ece.eng.wayne.edu/~apandya/). Documentation available at [Repository Wiki Page](https://github.com/akafael/LexyRobot/wiki) 5 | 6 | ## Folder Organization 7 | - [matlab](matlab) 8 | - [letters](matlab/letters): Generated handwriting letters .mat files 9 | - [pen](matlab/pen): Pen Script files 10 | - [lexyRobot.m](matlab/lexyRobot.mat) : Robot Class 11 | - [LexyRobotGUI.fig](matlab/LexyRobotGUI.fig) : Matlab GUI Figure file 12 | - [LexyRobotGUI.m](matlab/LexyRobotGUI.m) : Matlab GUI Script file 13 | - [testLexyRobot.m](matlab/testLexyRobot.m) : Matlab Script for unitary function testing 14 | 15 | ## LexyRobot GUI 16 | 17 | ![LexyRobotGUI](https://raw.githubusercontent.com/akafael/LexyRobot/master/doc/img/lexyRobotGUI.png) 18 | 19 | 20 | ### Features GUI 21 | 22 | - Scan COM ports and perform Connection 23 | - Inverse and Direct Kinematics 24 | - Control trough angle and coordinates in cm 25 | - Draw Words 26 | - Path Planning (DStar and PRM) 27 | 28 | ## Robot Image 29 | 30 | ![LexyRobot Hardware](https://github.com/akafael/LexyRobot/raw/master/doc/img/lexyRobotHardware.png) 31 | 32 | ## Reference 33 | ### Software 34 | - [Matlab Robotic Toolbox](http://petercorke.com/Robotics_Toolbox.html) Open Source Toolbox created by [@petercorke](https://github.com/petercorke) 35 | - [ArduinoIO](https://www.mathworks.com/hardware-support/arduino-matlab.html?requestedDomain=www.mathworks.com) Arduino Legacy Support for Matlab 36 | - [Object Oriented Programing on Matlab](https://www.mathworks.com/company/newsletters/articles/introduction-to-object-oriented-programming-in-matlab.html) Concepts used in the project 37 | - [Pen](https://www.mathworks.com/matlabcentral/fileexchange/26225-pen---mouse-draw-input-template) Pen / Mouse draw input template modified for this project 38 | 39 | ### Related Videos 40 | - [Robotic Theory](https://www.youtube.com/watch?v=0yD3uBshJB0&list=PL65CC0384A1798ADF) 41 | - [Project Results](https://www.youtube.com/watch?v=L9-bt8S9pyk&list=PLLVv9YVxXD97kZRXK5z1hEUbqRAv6iZf2) 42 | -------------------------------------------------------------------------------- /matlab/testLexyRobot.m: -------------------------------------------------------------------------------- 1 | %% Test Function 2 | function mRobot = testLexyRobot() 3 | % Start Robotic Toolkit 4 | startup_rvc(); 5 | 6 | % Create Robot Object 7 | mRobot = lexyRobot(); 8 | mRobot.printName(); 9 | 10 | % Connect Arduino 11 | mRobot.robotArduino = mRobot.connectArduino('/dev/ttyACM0'); 12 | mRobot.isConnected2Arduino = true; 13 | 14 | % Start Simulation 15 | mRobot.robotModel.plot([pi/2 pi/2]); 16 | 17 | %testMove(mRobot); 18 | %testCalibrationMove(mRobot); 19 | %testInverseKinematics(mRobot,3,4); 20 | %testDrawSquare(mRobot); 21 | %testDrawWord(mRobot,'RAFAEL',3) 22 | %testSizeLetter(mRobot); 23 | testPlottingMap(); 24 | %testDrawingPath(mRobot); 25 | 26 | % Disconnect Arduino 27 | mRobot.disconnectArduino(); 28 | end 29 | 30 | function testMove(mRobot) 31 | % Test Rotine for the function move 32 | % Using Robotic Toolkit (Direct Use) 33 | % use mRobot.robotModel for the SerialLink object 34 | q= [0 0] 35 | mRobot.robotModel.plot(q) 36 | 37 | 38 | % Using Arduino (Direct Use) 39 | % use mRobot.robotArduino to access the Arduino functions 40 | try 41 | mRobot.robotArduino.servolink1.writePosition(q(1)/pi); 42 | mRobot.robotArduino.servolink2.writePosition(q(2)/pi); 43 | catch 44 | disp('Unable to move Arm!') 45 | end 46 | 47 | % Using Both combined 48 | q1 = [pi/2 pi/2]; 49 | mRobot.moveFast(q1); 50 | end 51 | 52 | function testCalibrationMove(mRobot) 53 | % Test Rotine to calibrate the Servo Motors 54 | q0 = [0 0]; 55 | q1 = [pi pi]; 56 | 57 | mRobot.move(q0); 58 | mRobot.move(q1) 59 | end 60 | 61 | function testInverseKinematics(mRobot,X,Y) 62 | q = mRobot.ikine(X,Y) 63 | mRobot.robotModel.plot(q); 64 | end 65 | 66 | function testDrawSquare(mRobot) 67 | shape = [1 1;1 2;2 2;2 1] 68 | 69 | % Plot Shape 70 | for i = 1:size(shape,1) 71 | q = mRobot.ikine(shape(i,1),shape(i,2)) 72 | mRobot.robotModel.plot(q); 73 | pause(0.5); 74 | end 75 | 76 | % Go Back to the Begining of shape 77 | q = mRobot.ikine(shape(1,1),shape(1,2)) 78 | mRobot.robotModel.plot(q); 79 | end 80 | 81 | function testDrawChar(mRobot, msg, scale) 82 | 83 | qz = [90 150]*pi/180; 84 | mRobot.moveSync(qz); 85 | 86 | char = msg(1,1); 87 | 88 | nameFile = ['Letters/letter',char,'.mat'] 89 | load(nameFile,'letter'); 90 | 91 | q = mRobot.robotModel.getpos(); 92 | posX = q(1,1); 93 | posY = q(1,2); 94 | 95 | letter(:,1) = scale*letter(:,1) + posX; 96 | letter(:,2) = scale*letter(:,2) + posY; 97 | letter(:,3) = mRobot.PEN_DOWN; 98 | 99 | mRobot.robotModel.plot(letter(:,1:2),'fps',12); 100 | 101 | mRobot.penUpDown(mRobot.PEN_DOWN); 102 | 103 | % Plot Shape 104 | for i = 1:size(letter,1) 105 | q = mRobot.ikine(letter(i,1),letter(i,2)); 106 | mRobot.moveFast(q); 107 | mRobot.penUpDown(letter(i,3)); 108 | end 109 | 110 | % mRobot.penUpDown(mRobot.PEN_UP); 111 | 112 | % % Move to the next Letter 113 | % finalLetter = zeros(2); 114 | % finalLetter(1) = max(letter(:,1)); 115 | % finalLetter(2) = min(letter(:,2)); 116 | % q = mRobot.ikine(finalLetter(1,1),finalLetter(1,2)); 117 | % mRobot.moveFast(q); 118 | end 119 | 120 | function testDrawWord(mRobot, msg, scale) 121 | for i = 1:size(msg,2); 122 | mRobot.drawLetter(msg(i),scale); 123 | mRobot.penUpDown(mRobot.PEN_UP); 124 | end 125 | end 126 | 127 | function testNormLetter(char) 128 | 129 | nameFile = ['Letters/letter',char,'.mat']; 130 | load(nameFile,'letter'); 131 | 132 | nletter = letter; 133 | 134 | x1 = min(letter(:,1));x2=max(letter(:,1)); nletter(:,1) = (letter(:,1)-x1)/(x2 -x1); 135 | y1 = min(letter(:,2));y2=max(letter(:,2)); nletter(:,2) = (letter(:,2)-y1)/(y2 -y1); 136 | P = (x2 - x1)/(y2-y1); 137 | 138 | nletter(:,1) = nletter(:,1)*P; 139 | 140 | letter = nletter; 141 | nameFile = ['Letters/nletter',char,'.mat']; 142 | save(nameFile,'letter'); 143 | end 144 | 145 | function testTransposeLetter(char) 146 | 147 | nameFile = ['Letters/letter',char,'.mat']; 148 | load(nameFile,'letter'); 149 | 150 | nletter = letter; 151 | 152 | x1 = min(letter(:,2));x2=max(letter(:,2)); nletter(:,2) = (letter(:,2)-x1)/(x2 -x1); 153 | y1 = min(letter(:,1));y2=max(letter(:,1)); nletter(:,1) = (letter(:,1)-y1)/(y2 -y1); 154 | P = (x2 - x1)/(y2-y1); 155 | 156 | nletter(:,1) = nletter(:,1)*P; 157 | 158 | letter = nletter; 159 | nameFile = ['Letters/nletter',char,'.mat']; 160 | save(nameFile,'letter'); 161 | end 162 | 163 | function testSizeLetter(mRobot) 164 | nchar = 3 165 | 166 | dx = 0; 167 | dy = 3; 168 | 169 | q = mRobot.robotModel.getpos(); 170 | T = mRobot.robotModel.fkine(q); 171 | 172 | posx = T(1,4); 173 | posy = T(2,4); 174 | 175 | for i=1:nchar 176 | mRobot.penUpDown(mRobot.PEN_DOWN); 177 | posx = posx + dx; 178 | posy = posy + dy; 179 | newQ = mRobot.ikine(posx, posy) 180 | mRobot.penUpDown(mRobot.PEN_UP); 181 | pause(0.2) 182 | mRobot.moveFast(newQ) 183 | end 184 | end 185 | 186 | function testNormAll() 187 | abc = 'ABCDEFGHILMNOPQRSTUVWXYZ' 188 | 189 | for i = 1:size(abc,2) 190 | testNormLetter(abc(i)); 191 | end 192 | end 193 | 194 | function testPlottingMap() 195 | map = makemap(20) 196 | 197 | ds = Dstar(map); 198 | figure; 199 | ds.plot(); 200 | end 201 | 202 | function testDrawingPath(mRobot) 203 | % Create Map 204 | res = 20 205 | map = makemap(res) 206 | 207 | % Calculate Path base on this coordinates 208 | start = [2,18] 209 | goal = [18,2] 210 | 211 | ds = Dstar(map); 212 | ds.plan(goal); 213 | 214 | figure; 215 | path = ds.path(start); 216 | 217 | posX = 1; 218 | posY = 1; 219 | scale = 5/res 220 | 221 | q = mRobot.ikine(posX,posY); 222 | mRobot.penUpDown(mRobot.PEN_UP); 223 | mRobot.robotModel.plot(mRobot.robotPos); 224 | mRobot.moveFast(q); 225 | 226 | mRobot.robotToolkitDisp(['Drawing Path ',char]); 227 | 228 | path(:,1) = scale*path(:,1) + posX 229 | path(:,2) = scale*path(:,2) + posY 230 | 231 | mRobot.drawCurve(path); 232 | end 233 | -------------------------------------------------------------------------------- /matlab/pen/pen.m: -------------------------------------------------------------------------------- 1 | function pen 2 | % Pen-Like data processing template 3 | % pen.m is a GUI ready to use 4 | % the GUI calls a function called "process_data" 5 | % located at the end of the file every time the pen passes over the red area 6 | % include your processing code there... ! 7 | % 8 | % (c) 2009 Grup de Robotica de la UdL 9 | % http://robotica.udl.es 10 | global estat_pulsacio 11 | 12 | himatge = findobj('tag','PEN'); 13 | if (isempty(himatge)) 14 | estat_pulsacio = 0; 15 | 16 | % crear la nueva figura 17 | himatge = figure; % es genera una figura i li assigna un identificador 18 | 19 | hvisio.versio = '0.01'; 20 | 21 | set(himatge,'numbertitle','off'); % treu el numero de figura 22 | set(himatge,'name',['PEN (' hvisio.versio ')']); % nom 23 | set(himatge,'MenuBar','none'); % treiem el menu d'icons 24 | set(himatge,'doublebuffer','on'); % dos buffers de grafics 25 | set(himatge,'tag','PEN'); % identifiquem la figura 26 | set(himatge,'Color',[0.95 0.95 0.95]); 27 | set(himatge,'WindowButtonMotionFcn',@moviment); 28 | set(himatge,'WindowButtonDownFcn',@moviment_down); 29 | set(himatge,'WindowButtonUpFcn',@moviment_up); 30 | 31 | % crear l'axis 32 | h_axes = axes('position', [0 0 1 0.85]); 33 | set(h_axes,'Tag','AXES'); 34 | box(h_axes,'on'); 35 | grid(h_axes,'on'); 36 | axis(h_axes,[0 1 0 1]); 37 | axis(h_axes,'off'); 38 | hold(h_axes,'on'); 39 | 40 | % crear les marques de borrat 41 | fill([0 0.1 0.1 0 0],[0 0 0.1 0.1 0],'r'); 42 | fill([0.9 1 1 0.9 0.9],[0 0 0.1 0.1 0],'r'); 43 | 44 | % crear el text 45 | h_text = uicontrol('Style','edit','Units','normalized','Position',[0 0.85 1 0.15],'FontSize',14,'HorizontalAlignment','left','Enable','inactive','Tag','TEXT'); 46 | 47 | % ###### MENU ###################################### 48 | h_opt = uimenu('Label','&Options'); 49 | uimenu(h_opt,'Label','Manual mode {input processed at the red area}','Enable','off'); 50 | uimenu(h_opt,'Label','Clear / Restore','Callback',@mostrar); 51 | h_ref = uimenu(h_opt,'Label','References','separator','on'); 52 | uimenu(h_ref,'Label','Rectangular','Callback',@referencies,'Tag','NORMAL','UserData','REF'); 53 | uimenu(h_ref,'Label','Italic','Callback',@referencies,'Tag','ITALIC','UserData','REF'); 54 | uimenu(h_ref,'Label','None','Callback',@referencies,'Tag','CAP','UserData','REF','Checked','on'); 55 | 56 | uimenu(h_opt,'Label','Sortir - Exit','Callback','closereq;','separator','on'); 57 | 58 | h_opt2 = uimenu('Label','&About pen.m'); 59 | uimenu(h_opt2,'Label','Grup de Robotica de la Universitat de Lleida'); 60 | uimenu(h_opt2,'Label','Grupo de Robotica de la Universidad de Lleida'); 61 | uimenu(h_opt2,'Label','Robotic Team, University of Lleida (Spain)'); 62 | uimenu(h_opt2,'Label','Browse -> http://robotica.udl.cat','Callback','web http://robotica.udl.cat -browser','separator','on'); 63 | 64 | else 65 | figure(himatge); 66 | end 67 | % ######################################################################### 68 | 69 | % ######################################################################### 70 | function mostrar(hco,eventStruct) 71 | % seleccionar mode 72 | global x_pen y_pen 73 | 74 | % borrar dibuix anterior 75 | delete(findobj('Tag','TRAJECTORIA')); 76 | delete(findobj('Tag','TRAJECTORIA2')); 77 | 78 | % borrar dades anteriors 79 | x_pen = []; 80 | y_pen = []; 81 | 82 | % per si fa falta 83 | himatge = findobj('tag','PEN'); 84 | 85 | set(himatge,'WindowButtonMotionFcn',@moviment); 86 | set(himatge,'WindowButtonDownFcn',@moviment_down); 87 | set(himatge,'WindowButtonUpFcn',@moviment_up); 88 | % ######################################################################### 89 | 90 | % ######################################################################### 91 | function referencies(hco,eventStruct) 92 | % marcar referencies 93 | 94 | set(findobj('UserData','REF'),'Checked','off'); 95 | set(findobj('Tag',get(hco,'Tag')),'Checked','on'); 96 | 97 | % borrar anteriors 98 | delete(findobj('Tag','REFERENCIES')); 99 | 100 | % dibuixar les noves 101 | h_axes = findobj('Tag','AXES'); 102 | 103 | switch get(hco,'Tag') 104 | case {'NORMAL'} 105 | for i = 0:0.1:1 106 | % horitzontal 107 | plot(h_axes,[0 1],[i i],'k:','Color',[0.5 0.5 0.5],'Tag','REFERENCIES'); 108 | 109 | % vertical 110 | plot(h_axes,[i i],[0 1],'k:','Color',[0.5 0.5 0.5],'Tag','REFERENCIES'); 111 | end 112 | 113 | case {'ITALIC'} 114 | for i = 0:0.1:1 115 | % horitzontal 116 | plot(h_axes,[0 1],[i i],'k:','Color',[0.5 0.5 0.5],'Tag','REFERENCIES'); 117 | 118 | % vertical 119 | plot(h_axes,[i i+0.1],[0 1],'k:','Color',[0.5 0.5 0.5],'Tag','REFERENCIES'); 120 | end 121 | end 122 | % ######################################################################### 123 | 124 | % ######################################################################### 125 | function moviment_down(hco,eventStruct) 126 | % assegurar que no hi ha cap captura en marxa 127 | global estat_pulsacio x_pen y_pen 128 | 129 | % toggle 130 | estat_pulsacio = 1; 131 | 132 | % recuperar punt 133 | h_axes = findobj('Tag','AXES'); 134 | p = get(h_axes,'CurrentPoint'); 135 | x = p(1,1); 136 | y = p(1,2); 137 | 138 | % dades acumulatives 139 | x_pen = [x_pen x]; 140 | y_pen = [y_pen y]; 141 | 142 | % dibuixar 143 | plot(h_axes,x,y,'b.','Tag','TRAJECTORIA'); 144 | % ######################################################################### 145 | 146 | % ######################################################################### 147 | function moviment_up(hco,eventStruct) 148 | % assegurar que no hi ha cap captura en marxa 149 | global estat_pulsacio x_pen y_pen 150 | 151 | % toggle 152 | estat_pulsacio = 0; 153 | 154 | h_axes = findobj('Tag','AXES'); 155 | 156 | % analisi del que s'ha polsat 157 | % borrar requadre anterior 158 | delete(findobj('Tag','TRAJECTORIA2')); 159 | 160 | % marcar un requadre 161 | x_i = min(x_pen); 162 | x_f = max(x_pen); 163 | x_d = max([1 (x_f - x_i)]); 164 | y_i = min(y_pen); 165 | y_f = max(y_pen); 166 | y_d = max([1 (y_f - y_i)]); 167 | plot(h_axes,[x_i x_f x_f x_i x_i],[y_i y_i y_f y_f y_i],'K:','MarkerSize',22,'Tag','TRAJECTORIA2'); 168 | % ######################################################################### 169 | 170 | % ######################################################################### 171 | function moviment(hco,eventStruct) 172 | % asegurar que no hi ha cap captura en marxa 173 | global estat_pulsacio x_pen y_pen 174 | 175 | h_axes = findobj('Tag','AXES'); 176 | 177 | p = get(h_axes,'CurrentPoint'); 178 | x = p(1,1); 179 | y = p(1,2); 180 | 181 | if estat_pulsacio 182 | % boto pulsat 183 | 184 | if ((y < 0) || (y > 1) || (x < 0) || (x > 1)) 185 | % no fer res 186 | return; 187 | end 188 | 189 | if ((x ~= x_pen(end)) || (y ~= y_pen(end))) 190 | % punt seguent 191 | x_pen = [x_pen x]; 192 | y_pen = [y_pen y]; 193 | 194 | plot(h_axes,[x_pen(end-1) x],[y_pen(end-1) y],'b.-','Tag','TRAJECTORIA'); 195 | end 196 | 197 | else 198 | 199 | if ((x < 0.1) && (y < 0.1)) || ((x > 0.9) && (y < 0.1)) 200 | % al passar sobre la zona roja 201 | if (~isempty(x_pen)) 202 | 203 | % processar la paraula 204 | process_data(x_pen,y_pen); 205 | 206 | % borrar dibuix 207 | delete(findobj('Tag','TRAJECTORIA')); 208 | delete(findobj('Tag','TRAJECTORIA2')); 209 | 210 | % borrar dades 211 | x_pen = []; 212 | y_pen = []; 213 | end 214 | end 215 | end 216 | % ######################################################################### 217 | 218 | % ######################################################################### 219 | function process_data(x_pen,y_pen) 220 | persistent num_blocks 221 | 222 | if isempty(num_blocks) 223 | num_blocks = 1; 224 | else 225 | num_blocks = num_blocks + 1; 226 | end 227 | 228 | % your code for word recognition / draw processing starts here 229 | % x_pen, y_pen are the current point locations 230 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 231 | 232 | 233 | set(findobj('Tag','TEXT'),'String',[' ' num2str(num_blocks) ' inputs processed']); 234 | % ######################################################################### 235 | -------------------------------------------------------------------------------- /matlab/lexyRobot.m: -------------------------------------------------------------------------------- 1 | %% Class to represent control an two link robot 2 | classdef lexyRobot 3 | % LEXY_ROBOT functions for robot 4 | 5 | properties 6 | name = 'Lexy Robot' % Robot Name 7 | robotModel; % Model Representation 8 | robotArduino; % Harware Connection 9 | mArduino; % Arduino Library 10 | connectionPort; % Connection Port 11 | this % Reference for the object 12 | 13 | %% DH Parameters (inch, radians) 14 | % dhLink1 = struct('a',4.534 , 'd',1.86 , 'alpha',0); 15 | % dhLink2 = struct('a',4.225 , 'd',0.71 , 'alpha',0); 16 | 17 | %% DH Parameters (cm , radians) 18 | dhLink1 = struct('a',11.51 , 'd',4.72 , 'alpha',0); 19 | dhLink2 = struct('a',10.75 , 'd',1.80 , 'alpha',0); 20 | 21 | %% DH Parameters (mm , radians) 22 | % dhLink1 = struct('a',115.1 , 'd',47.2 , 'alpha',0); 23 | % dhLink2 = struct('a',107.5 , 'd',18.0 , 'alpha',0); 24 | 25 | %% Robot State Variables 26 | robotPos = [pi/2 pi/2]; % Current joint angles 27 | penState = 1; % Current Pen State 28 | isConnected2Arduino = false; % Status Connection 29 | end 30 | 31 | properties(Constant=true) 32 | %% Calibration Constants 33 | LINK1_PI = pi/0.9; 34 | LINK2_PI = pi/0.9; 35 | 36 | MAX_SPEED_SERVO = (pi/3)/0.19; % rad/s 37 | DT = 0.007; 38 | 39 | ANGLE_PEN_UP = 0.4; 40 | ANGLE_PEN_DOWN = 0.9; 41 | PEN_UP = 1; 42 | PEN_DOWN = 0; 43 | 44 | %% Debug Constants 45 | 46 | end 47 | 48 | methods 49 | function obj = lexyRobot() 50 | obj.robotModel = obj.createModel(); 51 | obj.penState = obj.PEN_UP; 52 | % obj.robotArduino = obj.connectArduino('/dev/ttyACM0'); 53 | % obj.robotModel.plot(obj.robotPos); 54 | % obj.moveFast(obj.robotPos); 55 | end 56 | 57 | function printName(obj) 58 | disp(obj.name) 59 | end 60 | 61 | function moveSync(obj,q1) 62 | %% Move Simulation and Robot synchronized 63 | 64 | % Check if the angle is allowed for the robot 65 | if((sum(q1<0) + sum(q1>pi) ) >0) 66 | msgID = 'ROBOT:AngleOutsideLimit'; 67 | msg = '\tValue outside the joint limit. The angle must be between 0 and pi'; 68 | angleException = MException(msgID,msg); 69 | throw(angleException); 70 | end 71 | 72 | % Generate Trajetory: 73 | q0 = obj.robotModel.getpos(); 74 | N = round(max(abs(q1-q0))/(obj.MAX_SPEED_SERVO*obj.DT)); 75 | qs = jtraj(q0,q1,N); 76 | 77 | for i = 2:N 78 | q = qs(i,1:2); 79 | dt = max(abs(qs(i,1:2)-qs(i-1,1:2)))/obj.MAX_SPEED_SERVO; 80 | 81 | % Plot 82 | obj.robotModel.plot(q); 83 | 84 | % Move servos 85 | if(obj.isConnected2Arduino) 86 | try 87 | obj.robotArduino.servolink1.writePosition(... 88 | (pi - q(1))/obj.LINK1_PI); 89 | obj.robotArduino.servolink2.writePosition(... 90 | (pi - q(2))/obj.LINK2_PI); 91 | 92 | q0(1) = obj.robotArduino.servolink1.readPosition(); 93 | q0(2) = obj.robotArduino.servolink2.readPosition(); 94 | 95 | obj.robotPos = q; 96 | catch ME 97 | obj.arduinoDisp('Unable to move Servos'); 98 | rethrow(ME); 99 | end 100 | end 101 | 102 | pause(dt); 103 | end 104 | end 105 | 106 | function moveFast(obj,q) 107 | %% Plot final position and then move robot 108 | 109 | % Check if the angle is allowed for the robot 110 | if((sum(q<0) + sum(q>pi) ) >0) 111 | msgID = 'ROBOT:AngleOutsideLimit'; 112 | msg = '\tValue outside the joint limit. The angle must be between 0 and pi'; 113 | angleException = MException(msgID,msg); 114 | throw(angleException); 115 | end 116 | 117 | % Generate Trajetory: 118 | q0 = obj.robotModel.getpos(); 119 | qs = jtraj(q0,q,3); 120 | 121 | % Plot 122 | obj.robotModel.plot(qs,'fps',60); 123 | 124 | % Move Servos 125 | if(obj.isConnected2Arduino) 126 | try 127 | obj.robotArduino.servolink1.writePosition(... 128 | (pi - q(1))/obj.LINK1_PI); 129 | obj.robotArduino.servolink2.writePosition(... 130 | (pi - q(2))/obj.LINK2_PI); 131 | 132 | q0(1) = obj.robotArduino.servolink1.readPosition(); 133 | q0(2) = obj.robotArduino.servolink2.readPosition(); 134 | 135 | obj.robotPos = q; 136 | catch ME 137 | obj.arduinoDisp('Unable to move Servos'); 138 | rethrow(ME); 139 | end 140 | end 141 | end 142 | 143 | function penState = penUpDown(obj, state) 144 | if(obj.isConnected2Arduino) 145 | if state==obj.PEN_UP 146 | penState = obj.PEN_UP; 147 | obj.robotArduino.servoActuator.writePosition(obj.ANGLE_PEN_UP); 148 | else 149 | penState = obj.PEN_DOWN; 150 | obj.robotArduino.servoActuator.writePosition(obj.ANGLE_PEN_DOWN); 151 | end 152 | end 153 | end 154 | 155 | %% Arduino Functions 156 | function robotArduino = connectArduino(obj,port) 157 | %% Create the connection with the robot 158 | 159 | % Connect With Arduino 160 | obj.arduinoDisp(['Connecting with port ',port, '...']); 161 | 162 | try 163 | delete(instrfind({'Port'},{port})); 164 | obj.mArduino = arduino(port, 'Uno', 'Libraries', {'Servo'}); 165 | obj.connectionPort = port; 166 | 167 | % Attach Servos 168 | obj.robotArduino.servolink2 = servo(obj.mArduino,9); 169 | obj.robotArduino.servolink1 = servo(obj.mArduino,10); 170 | obj.robotArduino.servoActuator = servo(obj.mArduino,11); 171 | 172 | % Move to initial state 173 | q = obj.robotPos; 174 | obj.robotArduino.servolink1.writePosition(... 175 | (pi - q(1))/obj.LINK1_PI); 176 | obj.robotArduino.servolink2.writePosition(... 177 | (pi - q(2))/obj.LINK2_PI); 178 | obj.penUpDown(obj.PEN_UP); 179 | 180 | % Show message 181 | obj.arduinoDisp([obj.name,' Connected! =D']); 182 | 183 | % Return reference for servo motors 184 | robotArduino = obj.robotArduino; 185 | catch ME 186 | obj.arduinoDisp('Connection Error!'); 187 | robotArduino = []; 188 | 189 | if(obj.isConnected2Arduino) 190 | rethrow(ME); 191 | end 192 | end 193 | end 194 | 195 | function disconnectArduino(obj) 196 | %% Disconnect the robot 197 | delete(instrfind({'Port'},{obj.connectionPort})); 198 | obj.arduinoDisp([obj.name,' Disconnected! =O']); 199 | end 200 | 201 | %% Robotic Toolkit Functions 202 | function model= createModel(obj) 203 | %% Model Representation using Robotic Toolkit 204 | 205 | obj.robotToolkitDisp('Creating model for Control and Simmulation') 206 | 207 | % Joints Description 208 | LINK(1) = Revolute('d', obj.dhLink1.d, 'a', obj.dhLink1.a,... 209 | 'alpha', obj.dhLink1.alpha, 'm', 0, 'offset',-pi/2, ... 210 | 'qlim', [0 pi]); % kinematic: joint variable limits 211 | 212 | LINK(2) = Revolute('d', obj.dhLink2.d, 'a', obj.dhLink2.a,... 213 | 'alpha', obj.dhLink2.alpha, 'm', 0, 'offset',0, ... 214 | 'qlim', [0 pi]); % kinematic: joint variable limits 215 | 216 | % Create the Robot Model 217 | obj.robotModel = SerialLink(LINK, 'name', obj.name); 218 | obj.robotToolkitDisp(['Model for ',obj.robotModel.name,' created ']); 219 | 220 | model = obj.robotModel; 221 | end 222 | 223 | function drawLetter(obj,char,scale) 224 | % Get the current position 225 | q = obj.robotModel.getpos(); 226 | T = obj.robotModel.fkine(q); 227 | posX = T(1,4); 228 | posY = T(2,4); 229 | 230 | if char == ' ' 231 | dx = 0.5 232 | q = obj.ikine(posX+dx,posY); 233 | obj.penUpDown(obj.PEN_UP); 234 | obj.moveFast(q); 235 | else 236 | obj.robotToolkitDisp(['Writing letter ',char]); 237 | nameFile = ['Letters/nletter',char,'.mat']; 238 | load(nameFile,'letter'); 239 | 240 | letter(:,1) = scale*letter(:,1) + posX; 241 | letter(:,2) = scale*letter(:,2) + posY; 242 | letter(:,3) = obj.PEN_DOWN; 243 | 244 | % Plot Shape 245 | for i = 1:size(letter,1) 246 | q = obj.ikine(letter(i,1),letter(i,2)); 247 | obj.moveFast(q); 248 | obj.penUpDown(letter(i,3)); 249 | end 250 | 251 | % Prepare for the next letter 252 | posX = max(letter(:,1)); 253 | posY = min(letter(:,2)); 254 | q = obj.ikine(posX,posY); 255 | % obj.penUpDown(obj.PEN_UP); 256 | obj.moveFast(q); 257 | end 258 | end 259 | 260 | function plotLetter(obj,char,handles) 261 | if char == ' ' 262 | 263 | else 264 | fig = gca; 265 | axes(handles); 266 | nameFile = ['Letters/nletter',char,'.mat']; 267 | load(nameFile,'letter'); 268 | 269 | plot(letter(:,1)',letter(:,2)','b'); 270 | axis off; 271 | axes(fig); 272 | end 273 | end 274 | 275 | function drawWord(obj,msg,scale) 276 | % msg String to Draw 277 | % scale letter height 278 | for i = 1:size(msg,2); 279 | obj.drawLetter(msg(i),scale); 280 | obj.penUpDown(obj.PEN_UP); 281 | q = obj.robotModel.getpos(); 282 | obj.moveFast(q); 283 | end 284 | end 285 | 286 | function drawCurve(obj,curve) 287 | obj.robotToolkitDisp(['Preparing...',char]); 288 | obj.penUpDown(obj.PEN_UP); 289 | 290 | obj.robotToolkitDisp(['Drawing curve',char]); 291 | % Plot Curve 292 | for i = 1:size(curve,1) 293 | q = obj.ikine(curve(i,1),curve(i,2)); 294 | obj.moveFast(q); 295 | obj.penUpDown(obj.PEN_DOWN); 296 | end 297 | 298 | obj.penUpDown(obj.PEN_UP); 299 | end 300 | 301 | function q1 = ikine(obj,X,Y) 302 | %% Inverse Kinematic adapted for this robot 303 | 304 | % Get Current postion as initial condition 305 | qz = obj.robotPos; 306 | Tz = obj.robotModel.fkine(qz); 307 | 308 | % Generate New Position 309 | T1 = rt2tr(t2r(Tz),[X Y Tz(3,4)]'); 310 | 311 | % Create New Position Using Ikine 312 | M = [1 1 0 0 0 0]; 313 | q1 = obj.robotModel.ikine(T1,qz,M); 314 | end 315 | 316 | %% Custom Messages 317 | function arduinoDisp(obj,text) 318 | %% Message Output for Arduino Events 319 | disp(['[Arduino]: ',text]); 320 | end 321 | 322 | function robotToolkitDisp(obj,text) 323 | %% Message Output for Robot Toolkit Events 324 | disp(['[Robotic Toolkit]: ',text]); 325 | end 326 | end 327 | end -------------------------------------------------------------------------------- /matlab/LexyRobotGUI.m: -------------------------------------------------------------------------------- 1 | function varargout = LexyRobotGUI(varargin) 2 | %LEXYROBOTGUI M-file for LexyRobotGUI.fig 3 | % LEXYROBOTGUI, by itself, creates a new LEXYROBOTGUI or raises the existing 4 | % singleton*. 5 | % 6 | % H = LEXYROBOTGUI returns the handle to a new LEXYROBOTGUI or the handle to 7 | % the existing singleton*. 8 | % 9 | % LEXYROBOTGUI('Property','Value',...) creates a new LEXYROBOTGUI using the 10 | % given property value pairs. Unrecognized properties are passed via 11 | % varargin to LexyRobotGUI_OpeningFcn. This calling syntax produces a 12 | % warning when there is an existing singleton*. 13 | % 14 | % LEXYROBOTGUI('CALLBACK') and LEXYROBOTGUI('CALLBACK',hObject,...) call the 15 | % local function named CALLBACK in LEXYROBOTGUI.M with the given input 16 | % arguments. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | 23 | % Edit the above text to modify the response to help LexyRobotGUI 24 | 25 | % Last Modified by GUIDE v2.5 11-Dec-2015 01:33:14 26 | 27 | % Begin initialization code - DO NOT EDIT 28 | gui_Singleton = 1; 29 | gui_State = struct('gui_Name', mfilename, ... 30 | 'gui_Singleton', gui_Singleton, ... 31 | 'gui_OpeningFcn', @LexyRobotGUI_OpeningFcn, ... 32 | 'gui_OutputFcn', @LexyRobotGUI_OutputFcn, ... 33 | 'gui_LayoutFcn', [], ... 34 | 'gui_Callback', []); 35 | if nargin && ischar(varargin{1}) 36 | gui_State.gui_Callback = str2func(varargin{1}); 37 | end 38 | 39 | if nargout 40 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 41 | else 42 | gui_mainfcn(gui_State, varargin{:}); 43 | end 44 | % End initialization code - DO NOT EDIT 45 | 46 | 47 | % --- Executes just before LexyRobotGUI is made visible. 48 | function LexyRobotGUI_OpeningFcn(hObject, eventdata, handles, varargin) 49 | % This function has no output args, see OutputFcn. 50 | % hObject handle to figure 51 | % eventdata reserved - to be defined in a future version of MATLAB 52 | % handles structure with handles and user data (see GUIDATA) 53 | % varargin unrecognized PropertyName/PropertyValue pairs from the 54 | % command line (see VARARGIN) 55 | 56 | % Choose default command line output for LexyRobotGUI 57 | handles.output = hObject; 58 | 59 | %% Run robotic toolkit 60 | try 61 | startup_rvc; 62 | catch ME 63 | errordlg('Robotic Toolkit not Find'); 64 | rethrow ME; 65 | end 66 | 67 | %% Create Robot 68 | handles.mRobot = lexyRobot(); 69 | 70 | %% Prepare GUI 71 | handles.mRobot.isConnected2Arduino = false; % Connection Status Flag 72 | movegui(hObject,'center'); % Change GUI position 73 | axes(handles.axes1); % Chose Axis 74 | 75 | serialPorts = instrhwinfo('serial'); 76 | nPorts = length(serialPorts.SerialPorts); 77 | set(handles.menuportarduino, 'String', ... 78 | [{'Select a port'} ; serialPorts.SerialPorts ]); 79 | 80 | % Map 81 | handles.sizeMap = 20; 82 | handles.map = []; 83 | 84 | % Path Planning 85 | handles.start = [0,0]; 86 | handles.goal = [0,0]; 87 | handles.zeroMap = [1,1] 88 | 89 | 90 | % Update handles structure 91 | guidata(hObject, handles); 92 | 93 | % UIWAIT makes LexyRobotGUI wait for user response (see UIRESUME) 94 | % uiwait(handles.figure1); 95 | 96 | 97 | % --- Outputs from this function are returned to the command line. 98 | function varargout = LexyRobotGUI_OutputFcn(hObject, eventdata, handles) 99 | % varargout cell array for returning output args (see VARARGOUT); 100 | % hObject handle to figure 101 | % eventdata reserved - to be defined in a future version of MATLAB 102 | % handles structure with handles and user data (see GUIDATA) 103 | 104 | % Get default command line output from handles structure 105 | varargout{1} = handles.output; 106 | 107 | 108 | % --- Executes on button press in plot_graph. 109 | function plot_graph_Callback(hObject, eventdata, handles) 110 | % hObject handle to plot_graph (see GCBO) 111 | % eventdata reserved - to be defined in a future version of MATLAB 112 | % handles structure with handles and user data (see GUIDATA) 113 | 114 | % Get angles and convert to radians 115 | handles.jointangle1 = (pi/180)*str2double(get(handles.angle1,'String')); 116 | handles.jointangle2 = (pi/180)*str2double(get(handles.angle2,'String')); 117 | 118 | newQ = [handles.jointangle1 handles.jointangle2]; 119 | 120 | axes(handles.axes1); 121 | 122 | try 123 | set(handles.textRobotModel,'String','Moving Joints'); 124 | handles.mRobot.moveSync(newQ); 125 | set(handles.textRobotModel,'String','Done! =D'); 126 | catch ME 127 | errordlg('Unable to Move Robot'); 128 | set(handles.textRobotModel,'String','Error =('); 129 | end 130 | 131 | statusRobot_update(hObject, eventdata, handles); 132 | 133 | guidata(hObject, handles); 134 | 135 | % --- Executes on button press in animation. 136 | function animation_Callback(hObject, eventdata, handles) 137 | % hObject handle to animation (see GCBO) 138 | % eventdata reserved - to be defined in a future version of MATLAB 139 | % handles structure with handles and user data (see GUIDATA) 140 | angle1 = -pi/2:0.1:pi/2; 141 | angle2 = angle1- pi; 142 | 143 | newQ = [angle1' angle2']; 144 | 145 | axes(handles.axes1); 146 | 147 | for i = 1:size(newQ,1) 148 | 149 | handles.mRobot.robotModel.plot(newQ(i,:), 'fps', 10); 150 | 151 | statusRobot_update(hObject, eventdata, handles); 152 | 153 | pause(0.2); 154 | end 155 | 156 | % --- Executes on button press in btnConnectArduino. 157 | function btnConnectArduino_Callback(hObject, eventdata, handles) 158 | % hObject handle to btnConnectArduino (see GCBO) 159 | % eventdata reserved - to be defined in a future version of MATLAB 160 | % handles structure with handles and user data (see GUIDATA) 161 | 162 | if (handles.mRobot.isConnected2Arduino) 163 | handles.mRobot.disconnectArduino(); 164 | handles.mRobot.isConnected2Arduino = false; 165 | set(handles.btnConnectArduino,'String','Connect'); 166 | set(handles.textStatusConnectionArduino,'String', 'Not connected.'); 167 | else 168 | try 169 | serPortn = get(handles.menuportarduino, 'Value'); 170 | if serPortn == 1 171 | errordlg('Select valid COM port'); 172 | else 173 | serList = get(handles.menuportarduino,'String'); 174 | port = serList{serPortn}; 175 | 176 | set(handles.textStatusConnectionArduino,'String', ['Connecting...']); 177 | % Connect 178 | handles.mRobot.robotArduino = handles.mRobot.connectArduino(port); 179 | handles.mRobot.isConnected2Arduino = true; 180 | 181 | % Update Gui 182 | set(handles.textStatusConnectionArduino,'String', ['Connected in ',port]); 183 | set(handles.btnConnectArduino,'String','Disconect'); 184 | handles.mRobot.robotModel.plot(handles.mRobot.robotPos); 185 | statusRobot_update(hObject, eventdata, handles); 186 | set(handles.textRobotModel,'String', 'Simulation Ready!'); 187 | end 188 | catch ME 189 | set(handles.textStatusConnectionArduino,'String', 'Connection error!'); 190 | rethrow(ME); 191 | end 192 | end 193 | 194 | % Update handles structure 195 | guidata(hObject, handles); 196 | 197 | % --- Executes when user attempts to close figure1. 198 | function figure1_CloseRequestFcn(hObject, eventdata, handles) 199 | % hObject handle to figure1 (see GCBO) 200 | % eventdata reserved - to be defined in a future version of MATLAB 201 | % handles structure with handles and user data (see GUIDATA) 202 | 203 | if handles.mRobot.isConnected2Arduino 204 | % Disconect Arduino 205 | try 206 | handles.mRobot.disconnectArduino(); 207 | catch ME 208 | % rethrow ME 209 | end 210 | end 211 | % Hint: delete(hObject) closes the figure 212 | delete(hObject); 213 | 214 | 215 | % --- Executes on button press in btnCreateModel. 216 | function btnCreateModel_Callback(hObject, eventdata, handles) 217 | % hObject handle to btnCreateModel (see GCBO) 218 | % eventdata reserved - to be defined in a future version of MATLAB 219 | % handles structure with handles and user data (see GUIDATA) 220 | 221 | % Get the Default Position and plot 222 | newQ = handles.mRobot.robotPos; 223 | handles.mRobot.robotModel.plot(newQ); 224 | movegui(handles.output,'center'); 225 | 226 | statusRobot_update(hObject, eventdata, handles); 227 | 228 | function editwrite_Callback(hObject, eventdata, handles) 229 | % hObject handle to editwrite (see GCBO) 230 | % eventdata reserved - to be defined in a future version of MATLAB 231 | % handles structure with handles and user data (see GUIDATA) 232 | 233 | % Hints: get(hObject,'String') returns contents of editwrite as text 234 | % str2double(get(hObject,'String')) returns contents of editwrite as a double 235 | 236 | 237 | % --- Executes during object creation, after setting all properties. 238 | function editwrite_CreateFcn(hObject, eventdata, handles) 239 | % hObject handle to editwrite (see GCBO) 240 | % eventdata reserved - to be defined in a future version of MATLAB 241 | % handles empty - handles not created until after all CreateFcns called 242 | 243 | % Hint: edit controls usually have a white background on Windows. 244 | % See ISPC and COMPUTER. 245 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 246 | set(hObject,'BackgroundColor','white'); 247 | end 248 | 249 | function editposy_Callback(hObject, eventdata, handles) 250 | % hObject handle to editposy (see GCBO) 251 | % eventdata reserved - to be defined in a future version of MATLAB 252 | % handles structure with handles and user data (see GUIDATA) 253 | 254 | % Hints: get(hObject,'String') returns contents of editposy as text 255 | % str2double(get(hObject,'String')) returns contents of editposy as a double 256 | 257 | 258 | % --- Executes during object creation, after setting all properties. 259 | function editposy_CreateFcn(hObject, eventdata, handles) 260 | % hObject handle to editposy (see GCBO) 261 | % eventdata reserved - to be defined in a future version of MATLAB 262 | % handles empty - handles not created until after all CreateFcns called 263 | 264 | % Hint: edit controls usually have a white background on Windows. 265 | % See ISPC and COMPUTER. 266 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 267 | set(hObject,'BackgroundColor','white'); 268 | end 269 | 270 | 271 | 272 | function editposx_Callback(hObject, eventdata, handles) 273 | % hObject handle to editposy (see GCBO) 274 | % eventdata reserved - to be defined in a future version of MATLAB 275 | % handles structure with handles and user data (see GUIDATA) 276 | 277 | % Hints: get(hObject,'String') returns contents of editposy as text 278 | % str2double(get(hObject,'String')) returns contents of editposy as a double 279 | 280 | 281 | % --- Executes during object creation, after setting all properties. 282 | function editposx_CreateFcn(hObject, eventdata, handles) 283 | % hObject handle to editposy (see GCBO) 284 | % eventdata reserved - to be defined in a future version of MATLAB 285 | % handles empty - handles not created until after all CreateFcns called 286 | 287 | % Hint: edit controls usually have a white background on Windows. 288 | % See ISPC and COMPUTER. 289 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 290 | set(hObject,'BackgroundColor','white'); 291 | end 292 | 293 | function angle2_Callback(hObject, eventdata, handles) 294 | % hObject handle to angle2 (see GCBO) 295 | % eventdata reserved - to be defined in a future version of MATLAB 296 | % handles structure with handles and user data (see GUIDATA) 297 | 298 | % Hints: get(hObject,'String') returns contents of angle2 as text 299 | % str2double(get(hObject,'String')) returns contents of angle2 as a double 300 | 301 | 302 | % --- Executes during object creation, after setting all properties. 303 | function angle2_CreateFcn(hObject, eventdata, handles) 304 | % hObject handle to angle2 (see GCBO) 305 | % eventdata reserved - to be defined in a future version of MATLAB 306 | % handles empty - handles not created until after all CreateFcns called 307 | 308 | % Hint: edit controls usually have a white background on Windows. 309 | % See ISPC and COMPUTER. 310 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 311 | set(hObject,'BackgroundColor','white'); 312 | end 313 | 314 | % --- Executes on selection change in menuportarduino. 315 | function menuportarduino_Callback(hObject, eventdata, handles) 316 | % hObject handle to menuportarduino (see GCBO) 317 | % eventdata reserved - to be defined in a future version of MATLAB 318 | % handles structure with handles and user data (see GUIDATA) 319 | 320 | % Hints: contents = cellstr(get(hObject,'String')) returns menuportarduino contents as cell array 321 | % contents{get(hObject,'Value')} returns selected item from menuportarduino 322 | 323 | 324 | % --- Executes during object creation, after setting all properties. 325 | function menuportarduino_CreateFcn(hObject, eventdata, handles) 326 | % hObject handle to menuportarduino (see GCBO) 327 | % eventdata reserved - to be defined in a future version of MATLAB 328 | % handles empty - handles not created until after all CreateFcns called 329 | 330 | % Hint: popupmenu controls usually have a white background on Windows. 331 | % See ISPC and COMPUTER. 332 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 333 | set(hObject,'BackgroundColor','white'); 334 | end 335 | 336 | % --- Executes on button press in btnChangePos. 337 | function btnChangePos_Callback(hObject, eventdata, handles) 338 | % hObject handle to btnChangePos (see GCBO) 339 | % eventdata reserved - to be defined in a future version of MATLAB 340 | % handles structure with handles and user data (see GUIDATA) 341 | 342 | % Get angles and convert to radians 343 | handles.X = str2double(get(handles.editposx,'String')); 344 | handles.Y = str2double(get(handles.editposy,'String')); 345 | 346 | axes(handles.axes1); 347 | 348 | try 349 | set(handles.textRobotModel,'String',... 350 | ['Moving to (',num2str(handles.X),',',num2str(handles.Y),')']); 351 | newQ = handles.mRobot.ikine(handles.X, handles.Y); 352 | handles.mRobot.moveSync(newQ); 353 | statusRobot_update(hObject, eventdata, handles); 354 | set(handles.textRobotModel,'String', 'Done! =D'); 355 | catch ME 356 | errordlg('Unable to Move Robot'); 357 | set(handles.textRobotModel,'String','Error =('); 358 | rethrow(ME); 359 | end 360 | 361 | % --- Custom Function to update the Status Painel after each moviment 362 | function statusRobot_update(hObject, eventdata, handles) 363 | % hObject handle to btnChangePos (see GCBO) 364 | % eventdata reserved - to be defined in a future version of MATLAB 365 | % handles structure with handles and user data (see GUIDATA) 366 | 367 | newQ = handles.mRobot.robotModel.getpos(); 368 | 369 | set(handles.angle1,'String',num2str(round(newQ(1)*180/pi))); 370 | set(handles.angle2,'String',num2str(round(newQ(2)*180/pi))); 371 | 372 | finalTransformation = handles.mRobot.robotModel.fkine(newQ); 373 | 374 | x = finalTransformation(1,4); 375 | y = finalTransformation(2,4); 376 | z = finalTransformation(3,4); 377 | 378 | set(handles.xpos,'String',num2str(x)); 379 | set(handles.ypos,'String',num2str(y)); 380 | set(handles.zpos,'String',num2str(z)); 381 | 382 | set(handles.editposx,'String',num2str(x)); 383 | set(handles.editposy,'String',num2str(y)); 384 | 385 | finalOrientation = tr2rpy(finalTransformation, 'deg'); 386 | 387 | roll = finalOrientation(1); 388 | pitch = finalOrientation(2); 389 | yaw = finalOrientation(3); 390 | 391 | set(handles.rollVal,'String',num2str(roll)); 392 | set(handles.pitchVal,'String',num2str(pitch)); 393 | set(handles.yawVal,'String',num2str(yaw)); 394 | 395 | 396 | 397 | function angle1_Callback(hObject, eventdata, handles) 398 | % hObject handle to angle1 (see GCBO) 399 | % eventdata reserved - to be defined in a future version of MATLAB 400 | % handles structure with handles and user data (see GUIDATA) 401 | 402 | % Hints: get(hObject,'String') returns contents of angle1 as text 403 | % str2double(get(hObject,'String')) returns contents of angle1 as a double 404 | 405 | 406 | % --- Executes during object creation, after setting all properties. 407 | function angle1_CreateFcn(hObject, eventdata, handles) 408 | % hObject handle to angle1 (see GCBO) 409 | % eventdata reserved - to be defined in a future version of MATLAB 410 | % handles empty - handles not created until after all CreateFcns called 411 | 412 | % Hint: edit controls usually have a white background on Windows. 413 | % See ISPC and COMPUTER. 414 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 415 | set(hObject,'BackgroundColor','white'); 416 | end 417 | 418 | 419 | % --- Executes on button press in btnMovePen. 420 | function btnMovePen_Callback(hObject, eventdata, handles) 421 | % hObject handle to btnMovePen (see GCBO) 422 | % eventdata reserved - to be defined in a future version of MATLAB 423 | % handles structure with handles and user data (see GUIDATA) 424 | if handles.mRobot.penState == handles.mRobot.PEN_UP 425 | handles.mRobot.penState = ... 426 | handles.mRobot.penUpDown(handles.mRobot.PEN_DOWN); 427 | set(handles.btnMovePen, 'String', 'Pen Up'); 428 | else 429 | handles.mRobot.penState = handles.mRobot.penUpDown(handles.mRobot.PEN_UP); 430 | set(handles.btnMovePen, 'String', 'Pen Down'); 431 | end 432 | 433 | % Update handles structure 434 | guidata(hObject, handles); 435 | 436 | 437 | % --- Executes on button press in btnWrite. 438 | function btnWrite_Callback(hObject, eventdata, handles) 439 | % hObject handle to btnWrite (see GCBO) 440 | % eventdata reserved - to be defined in a future version of MATLAB 441 | % handles structure with handles and user data (see GUIDATA) 442 | 443 | % Get Message to write 444 | msg = get(handles.editwrite,'String'); 445 | msg = upper(msg); 446 | 447 | set(handles.textRobotModel,'String', 'Drawing message.'); 448 | 449 | for i = 1:size(msg,2); 450 | handles.mRobot.plotLetter(msg(i),handles.axesLetter); 451 | handles.mRobot.drawLetter(msg(i),1); 452 | handles.mRobot.penUpDown(handles.mRobot.PEN_UP); 453 | statusRobot_update(hObject, eventdata, handles); 454 | end 455 | 456 | % Update GUI 457 | set(handles.textRobotModel,'String', 'Done!'); 458 | set(handles.editwrite,'String',''); 459 | 460 | 461 | % --- Executes on button press in btnWriteName. 462 | function btnWriteName_Callback(hObject, eventdata, handles) 463 | % hObject handle to btnWriteName (see GCBO) 464 | % eventdata reserved - to be defined in a future version of MATLAB 465 | % handles structure with handles and user data (see GUIDATA) 466 | 467 | set(handles.textRobotModel,'String', 'Preparing...'); 468 | 469 | % Go for the left side of the page 470 | handles.X = -1; 471 | handles.Y = 4; 472 | 473 | handles.mRobot.penUpDown(handles.mRobot.PEN_UP); 474 | newQ = handles.mRobot.ikine(handles.X, handles.Y); 475 | handles.mRobot.moveSync(newQ); 476 | 477 | statusRobot_update(hObject, eventdata, handles); 478 | 479 | % Get Robot name 480 | msg = handles.mRobot.name; 481 | msg = upper(msg); 482 | 483 | % Write Robot Name 484 | set(handles.textRobotModel,'String', 'Writing robot name'); 485 | for i = 1:size(msg,2); 486 | handles.mRobot.plotLetter(msg(i),handles.axesLetter); 487 | handles.mRobot.drawLetter(msg(i),1); 488 | end 489 | 490 | % Update Status 491 | statusRobot_update(hObject, eventdata, handles); 492 | set(handles.textRobotModel,'String', 'Done'); 493 | 494 | 495 | % --- Executes during object creation, after setting all properties. 496 | function axesLetter_CreateFcn(hObject, eventdata, handles) 497 | % hObject handle to axesLetter (see GCBO) 498 | % eventdata reserved - to be defined in a future version of MATLAB 499 | % handles empty - handles not created until after all CreateFcns called 500 | 501 | % Hint: place code in OpeningFcn to populate axesLetter 502 | % hide axis 503 | % axis(hObject,'off'); 504 | 505 | 506 | % --- Executes on button press in btnPath. 507 | function btnPath_Callback(hObject, eventdata, handles) 508 | % hObject handle to btnPath (see GCBO) 509 | % eventdata reserved - to be defined in a future version of MATLAB 510 | % handles structure with handles and user data (see GUIDATA) 511 | 512 | mapFig = figure; 513 | 514 | try 515 | handles.pathmap.plot(); 516 | catch ME 517 | close(mapFig); 518 | errordlg('Map Not find!'); 519 | end 520 | 521 | % --- Executes on button press in btnMap. 522 | function btnMap_Callback(hObject, eventdata, handles) 523 | % hObject handle to btnMap (see GCBO) 524 | % eventdata reserved - to be defined in a future version of MATLAB 525 | % handles structure with handles and user data (see GUIDATA) 526 | 527 | % get current figure 528 | fig = gca; 529 | 530 | % Show help msg 531 | % msgbox({'left button, click and drag to create a rectangle';... 532 | % 'or type the following letters in the figure window:';... 533 | % 'p - draw polygon';... 534 | % 'c - draw circle';... 535 | % 'e - erase map';... 536 | % 'u - undo last action';... 537 | % 'q - leave editing mode'},'makemap'); 538 | 539 | % Open Map editor 540 | mapFig = figure; 541 | handles.map = makemap(handles.sizeMap); 542 | uiwait(mapFig); 543 | 544 | % Create a Basic Map 545 | handles.pathmap = PRM(handles.map); 546 | 547 | % Select previews Axis 548 | axes(fig); 549 | 550 | % Status message 551 | set(handles.textRobotModel,'String', 'Map Edited!'); 552 | 553 | % Update handles structure 554 | guidata(hObject, handles); 555 | 556 | 557 | % --- Executes on button press in btnDstart. 558 | function btnSetStartGoal_Callback(hObject, eventdata, handles) 559 | % hObject handle to btnDstart (see GCBO) 560 | % eventdata reserved - to be defined in a future version of MATLAB 561 | % handles structure with handles and user data (see GUIDATA) 562 | 563 | % Set Start Point 564 | startX = str2num(get(handles.editStartX,'String')); 565 | startY = str2num(get(handles.editStartY,'String')); 566 | handles.start = [startX,startY]; 567 | 568 | % Set Goal Point 569 | goalX = str2num(get(handles.editGoalX,'String')); 570 | goalY = str2num(get(handles.editGoalY,'String')); 571 | handles.goal = [goalX,goalY] - handles.zeroMap; 572 | 573 | set(handles.textRobotModel,'String', 'Start and Goal Defined!'); 574 | 575 | % Update handles structure 576 | guidata(hObject, handles); 577 | 578 | % --- Executes on button press in btnDstart. 579 | function btnDstart_Callback(hObject, eventdata, handles) 580 | % hObject handle to btnDstart (see GCBO) 581 | % eventdata reserved - to be defined in a future version of MATLAB 582 | % handles structure with handles and user data (see GUIDATA) 583 | % Calculate Path D* 584 | ds = Dstar(handles.map); 585 | ds.plan(handles.goal); 586 | handles.path = ds.path(handles.start) 587 | figure; 588 | ds.plot(); 589 | handles.ds = ds; 590 | 591 | handles.pathmap = handles.ds; 592 | 593 | set(handles.textRobotModel,'String', 'D* Path Created!'); 594 | 595 | % Update handles structure 596 | guidata(hObject, handles); 597 | 598 | % --- Executes on button press in btnPRM. 599 | function btnPRM_Callback(hObject, eventdata, handles) 600 | % hObject handle to btnPRM (see GCBO) 601 | % eventdata reserved - to be defined in a future version of MATLAB 602 | % handles structure with handles and user data (see GUIDATA) 603 | % Calculate Path PRM 604 | prm = PRM(handles.map); 605 | prm.plan(); 606 | handles.path = prm.path(handles.start,handles.goal) 607 | figure; 608 | prm.plot(); 609 | handles.prm = prm; 610 | 611 | set(handles.textRobotModel,'String', 'PRM Path Created!'); 612 | 613 | handles.pathmap = handles.prm; 614 | 615 | % Update handles structure 616 | guidata(hObject, handles); 617 | 618 | 619 | function editStartX_Callback(hObject, eventdata, handles) 620 | % hObject handle to editStartX (see GCBO) 621 | % eventdata reserved - to be defined in a future version of MATLAB 622 | % handles structure with handles and user data (see GUIDATA) 623 | 624 | % Hints: get(hObject,'String') returns contents of editStartX as text 625 | % str2double(get(hObject,'String')) returns contents of editStartX as a double 626 | 627 | 628 | % --- Executes during object creation, after setting all properties. 629 | function editStartX_CreateFcn(hObject, eventdata, handles) 630 | % hObject handle to editStartX (see GCBO) 631 | % eventdata reserved - to be defined in a future version of MATLAB 632 | % handles empty - handles not created until after all CreateFcns called 633 | 634 | % Hint: edit controls usually have a white background on Windows. 635 | % See ISPC and COMPUTER. 636 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 637 | set(hObject,'BackgroundColor','white'); 638 | end 639 | 640 | 641 | 642 | function editStartY_Callback(hObject, eventdata, handles) 643 | % hObject handle to editStartY (see GCBO) 644 | % eventdata reserved - to be defined in a future version of MATLAB 645 | % handles structure with handles and user data (see GUIDATA) 646 | 647 | % Hints: get(hObject,'String') returns contents of editStartY as text 648 | % str2double(get(hObject,'String')) returns contents of editStartY as a double 649 | 650 | 651 | % --- Executes during object creation, after setting all properties. 652 | function editStartY_CreateFcn(hObject, eventdata, handles) 653 | % hObject handle to editStartY (see GCBO) 654 | % eventdata reserved - to be defined in a future version of MATLAB 655 | % handles empty - handles not created until after all CreateFcns called 656 | 657 | % Hint: edit controls usually have a white background on Windows. 658 | % See ISPC and COMPUTER. 659 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 660 | set(hObject,'BackgroundColor','white'); 661 | end 662 | 663 | 664 | 665 | function editGoalX_Callback(hObject, eventdata, handles) 666 | % hObject handle to editGoalX (see GCBO) 667 | % eventdata reserved - to be defined in a future version of MATLAB 668 | % handles structure with handles and user data (see GUIDATA) 669 | 670 | % Hints: get(hObject,'String') returns contents of editGoalX as text 671 | % str2double(get(hObject,'String')) returns contents of editGoalX as a double 672 | 673 | 674 | % --- Executes during object creation, after setting all properties. 675 | function editGoalX_CreateFcn(hObject, eventdata, handles) 676 | % hObject handle to editGoalX (see GCBO) 677 | % eventdata reserved - to be defined in a future version of MATLAB 678 | % handles empty - handles not created until after all CreateFcns called 679 | 680 | % Hint: edit controls usually have a white background on Windows. 681 | % See ISPC and COMPUTER. 682 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 683 | set(hObject,'BackgroundColor','white'); 684 | end 685 | 686 | 687 | function editGoalY_Callback(hObject, eventdata, handles) 688 | % hObject handle to editGoalY (see GCBO) 689 | % eventdata reserved - to be defined in a future version of MATLAB 690 | % handles structure with handles and user data (see GUIDATA) 691 | 692 | % Hints: get(hObject,'String') returns contents of editGoalY as text 693 | % str2double(get(hObject,'String')) returns contents of editGoalY as a double 694 | 695 | 696 | % --- Executes during object creation, after setting all properties. 697 | function editGoalY_CreateFcn(hObject, eventdata, handles) 698 | % hObject handle to editGoalY (see GCBO) 699 | % eventdata reserved - to be defined in a future version of MATLAB 700 | % handles empty - handles not created until after all CreateFcns called 701 | 702 | % Hint: edit controls usually have a white background on Windows. 703 | % See ISPC and COMPUTER. 704 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 705 | set(hObject,'BackgroundColor','white'); 706 | end 707 | 708 | % --- Executes on button press in btnRunPath. 709 | function btnRunPath_Callback(hObject, eventdata, handles) 710 | % hObject handle to btnRunPath (see GCBO) 711 | % eventdata reserved - to be defined in a future version of MATLAB 712 | % handles structure with handles and user data (see GUIDATA) 713 | set(handles.textRobotModel,'String', 'Preparing...'); 714 | 715 | % Generate Path Curve 716 | scale = 1; 717 | 718 | path = handles.path; 719 | path(:,1) = scale*path(:,1) + handles.zeroMap(1); 720 | path(:,2) = scale*path(:,2) + handles.zeroMap(2); 721 | 722 | % Draw Path Curve 723 | set(handles.textRobotModel,'String', 'Drawing Path Curve.'); 724 | 725 | mapFig = figure; 726 | handles.pathmap.plot(); 727 | for i = 1:size(path,1) 728 | q = handles.mRobot.ikine(path(i,1),path(i,2)); 729 | handles.mRobot.moveFast(q); 730 | handles.mRobot.penUpDown(handles.mRobot.PEN_DOWN); 731 | statusRobot_update(hObject, eventdata, handles); 732 | end 733 | 734 | mapFig.delete(); 735 | 736 | % Update Status 737 | statusRobot_update(hObject, eventdata, handles); 738 | set(handles.textRobotModel,'String', 'Done'); 739 | --------------------------------------------------------------------------------