Robot_Class.m

classdef Robot_Class < handle
    %Robot_Class
    %   This class has methids to create and store a robot, as well as
    %   functions to do forward and inverse kinematics and plotting.
    
    properties
        Linked_Robot% Variable that holds the linked robot
        Thetas%Variable that holds the Thetas for each joint in the robot
        Controller%Variable that holds the arduino configuration
        Servos%Variable that holds the servo configurations
        Num_Joints;%Variabel that holds the number of joints in the robot
        Num_Servos;%Variable that holds the number of servos in the robot.
        Hardware_Used%Variable that holds whether hardware is attached to the robot
        Tool_Position%Variable that holds the position of the tool in XYZ space
        Joint_Direction %Variable used to tell which direction is clockwise and counterclockwise for each joint. We put these into the arduino_map function we
        %created and use them as the max and min for the output. If we flip these,
        %it effectively flips the direction of rotation for the servo
        Sensor_Pin='A0';
        LED_Pin='D5';
        Buzzer_Pin='D7';
        
    end
    
    methods
        function obj = Robot_Class(dh_parameters,Num_Links,Limits,Base,Tool)
            %INPUTS:
            %      dh_parameters: This is an array that specifies your DH
            %      parameters. It is the form of
            %      [d1,a1,alpha1,offset1 (if used),d2,a2,alpha2,offset2
            %      (if used),...]
            
            %      Num_Links: This is the number of links that the robot
            %      has
            
            %      Limits: These are a vector that contain the limits for
            %      each joint. Will be in the form of
            %      [min1,max1,min2,max2,...]
            
            %      Base: A 4x4 transformation matrix that relates an
            %      external reference frame to the first joint of the
            %      robot. Or it can be a 1x3 vector that contains the
            %      translation if the external frame has the same
            %      orientation as the first joint.
            
            %      Tool: A 4x4 transformation matrix that relates an
            %      tool frame to the last joint of the
            %      robot. Or it can be a 1x3 vector that contains the
            %      translation if the tool frame has the same
            %      orientation as the last joint.
            %END INPUTS
            
            %OUTPUTS
            %      obj: This is the obj that is created when the
            %      constructor has finished. It will hold the robot as
            %      well as the angles associated with each joint
            %END OUTPUTS
            if(nargin==2)%If there were only 2 input arguments
                for(i=1:2*Num_Links)% Loop through twice as many joints because there is a lower and upper limit for each joint
                    if(mod(i,2)==1)% If the joint number is odd, then it is a lower joint limit
                        Limits(i)=-90*pi/180;% Set it to -90 degrees
                    else% Otherwise it is an even joint and it becomes the upper limit
                        Limits(i)=90*pi/180;% Set the limit to 90 degrees
                    end
                end
                Base=[0,0,0];%The base transformation is defaulted to the same orientation as the first robot coordinate frame and has zero translation
                Tool=[0,0,0];%The tool transformation is defaulted to the same orientation as the last link with zero translational offset.
            end
            if(nargin==3)%If the Limits were defined but the base and tool transforms were not.
                Base=[0,0,0];%The base transformation is defaulted to the same orientation as the first robot coordinate frame and has zero translation
                Tool=[0,0,0];%The tool transformation is defaulted to the same orientation as the last link with zero translational offset.
            end
            if(nargin==4)%If the limits and the base were specified but not the tool
                Tool=[0,0,0];%The tool transformation is defaulted to the same orientation as the last link with zero translational offset.
            end
            
            if(mod(numel(dh_parameters),4) == 0) %Check to see if an offset was put into the dh parameters.
                Used_Offset = 1; %If the number of elements in the dh parameters was evenly divisible by 4, then an offset was used and we should indicate this with a variable
            else
                Used_Offset = 0;%Otherwise an offset was not used.
            end
            
            if(Used_Offset == 1)% If an offset value os included into the parameters
                for(i=1:1:Num_Links)
                    % Grab the values that correspond to the dh parameters of the link
                    % The argument of the dh_parameters is used to shift to the new link
                    %after each iteration has completed
                    L(i)=Revolute('d',dh_parameters(4*i-3),'a',dh_parameters(4*i-2),'alpha',dh_parameters(4*i-1),'offset',dh_parameters(4*i),'modified','qlim',Limits(2*i-1:2*i));
                end
            else
                for(i=1:1:Num_Links)
                    %Do the same thing as before excpet that no offset was included
                    L(i)=Revolute('d',dh_parameters(3*i-2),'a',dh_parameters(3*i-1),'alpha',dh_parameters(3*i),'modified','qlim',Limits(2*i-1:2*i));
                end
            end
            
            obj.Linked_Robot = SerialLink(L,'base',Base,'tool',Tool,'name', 'robot');%Link the joints together and sets the name of the robot
            obj.Thetas=zeros(1,Num_Links);% Set the default state for the Thetas
            obj.Num_Joints=Num_Links;% Set the number of joints in the robot.
            obj.Num_Servos=0;% Set the default number of servos to zero since we haven't attached any yet
            obj.Servos=[];% Start off with a blank array for the servos
            T=obj.Linked_Robot.fkine(obj.Thetas);% Do a forward kinematics to set the current tool position
            obj.Tool_Position=T.transl;% Set the current tool position to the previous forward kinematics
            obj.Joint_Direction= [1,0;1,0;1,0];
            clear T;
        end
        
        function DisplayRobotParameters(obj)
            %INPUTS
            %       obj: This is automatically passed in when the function
            %       is called
            %END INPUTS
            disp("Number of Joints: ")% Display the number of joints in the robot
            disp(obj.Num_Joints);
            
            disp("Joint DH parameters:");% Display the dh parameters of the robot
            obj.Linked_Robot.display()
            
            disp(" ");
            disp("Current Theta Angles: ");% Display the current joint angles
            disp(obj.Thetas*180/pi)
            
            
            disp (" ");
            disp("Hardware Configuration:")% Difsplay the hardware configuration(Arduino Setup)
            disp(obj.Controller)
            
            disp(" ");
            disp("Servo Configuration:");% Display the configuration of the servos attached to the robot
            for(i=1:1:obj.Num_Servos)% Loop through all the servos and display them individually
                disp(obj.Servos(i));
            end
        end
        
        function PlotRobot(obj,View_Angles)
            %INPUTS
            %       obj: This is automatically passed in when the function
            %       is called
            
            %       View_Angles: This is the set of viewing angles that are
            %       used to set the view window for the plot. If this is
            %       not specified, it will overwrite the current axes but
            %       keep the same view
            %END INPUTS
            
            %Notes: This will overwrite the current plot, make sure that
            %you call the correct axes or figure before writing, otherwise
            %you will get an error.
            
            if(nargin==2)%If the number of arguments is 2, then the user wants to specify a viewing angle
                obj.Linked_Robot.plot(obj.Thetas,'view', View_Angles);
            else
                obj.Linked_Robot.plot(obj.Thetas);%Otherwise, they just wanted a regular plot/to update the existing plot
            end
        end
        
        function Transformation=FkineRobot(obj)%Function to do the forward kinematics on the robot
            %INPUTS
            %       obj: This is automatically passed in when the function
            %       is called
            %END INPUTS
            Transformation=obj.Linked_Robot.fkine(obj.Thetas);%Return the transformation matrix from the fkine built within the robotics toolkit
            obj.Tool_Position=Transformation.transl;%Set the current tool position to the previous forward kinematics
        end
        
        function Angles=IkineRobot(obj,T,Mask,Initial_Angles)%Function to do the forward kinematics on the robot
            %INPUTS
            %       obj: This is automatically passed in when the function
            %       is called
            
            %       T: A 4x4 Transfomation matrix. Must be either a SE3
            %       object or a 4x4 homogeneous matrix
            %       Mask: This is used for when the robot has fewer than 6
            %       degrees of freedom. the mask for a 3 degree of freedom
            %       robot would look like [1,1,1,0,0,0]. A 2 degree of
            %       freedom robot will look like [1,1,0,0,0,0]
            
            %       Initial_Values: Vector that contains the initial
            %       guesses for the ikine. Must have the same length as the
            %       number of joints in the robot
            %END INPUTS
            %Angles=obj.Linked_Robot.ikunc(T,Initial_Values);%Return the transformation matrix from the fkine built within the robotics toolkit
            Minimized_Angles=obj.Linked_Robot.ikcon(T,Initial_Angles);
            Angles=obj.Linked_Robot.ikine(T,'mask',Mask,'q0',Minimized_Angles,'tol',5e-15);%Return the transformation matrix from the fkine built within the robotics toolkit
        end
        
        function [Points,s]=PlanPath(obj, start, goal, map,algorithm)% Function to calculate a path between the start and end points
            if(algorithm== 0)% If the value of algortihm passed in is zero, then we are going to use the A-star algorithm
                dx=DXform(map);% Create the A-star object
                dx.plan(goal);% calculate the costs to the goal
                Points=dx.query(start);% Plan the path from the starting location to the goal
            else% Otherwise, we are using the Probabilistic road map
                Points=[];
                prm=PRM(map);% Create the road map object
                prm.plan();% Throw the points in the space and connect them
                s=prm.query(start,goal);% Plan a path from the start to the goal
                for(i=1:numel(s)/2-1)
                   Pointsx = tpoly(s(i,1), s(i+1,1),10);
                   Pointsy = tpoly(s(i,2), s(i+1,2),10);
                   Points=[Points;Pointsx,Pointsy];
                end
            end
        end
        
        function Light_Value=GetLightSensorValue(obj)
                 Light_Value = readVoltage(obj.Controller,obj.Sensor_Pin);
        end
        
        function Checked_Angle=CheckLimits(obj,Joint_Number,angle)%Function to check if the angle passed in or the Thetas in the robot class are valid
            %INPUTS:
            %      obj: the object, you don't need to worry about this as
            %      it is automatically passed in
            
            %      Joint_Number: The number of the joint that we want to
            %      compare the angle against.
            
            %      angle: The angle that we are checking against the joint
            %      limits for the given joint number
            %END INPUTS
            
            %OUTPUTS
            %      Checked_Angle: This is the value that has been checked
            %      against the joint limits. It can be anywhere between
            %      the limits of the specififed joint
            %END OUTPUTS
            if(angle > obj.Linked_Robot.qlim(Joint_Number,2))%If the angle passed in was bigger than the maximum for that joint
                Checked_Angle = obj.Linked_Robot.qlim(Joint_Number,2);% Set the theta value to the maximum joint angle for that joint
                msgbox("The value entered was too large and was set to the max positive value");
                uiwait();
            elseif(angle < obj.Linked_Robot.qlim(Joint_Number,1))% If the angle passed in is less than the minimum joint angle for that joint
                Checked_Angle = obj.Linked_Robot.qlim(Joint_Number,1);% Set the theta to the minimum joint angle for that joint
                msgbox("The value entered was too small and was set to the max negative value")%Tell the user that the passed value exceeded the joint limit and was set to the joint limit
                uiwait();
            else
                Checked_Angle=angle;%The angle passed in was not out of bounds, so just pass it back through
            end
        end
        
        function T=MoveRobot(obj,X,Y,Z)% Function to move the robot. If hardware is not used then it will just plot the robot on the GUI
            
            T=obj.FkineRobot();% Create the current transformation matrix
            T.t(1)=X; %Change the value in the X spot to the new one
            T.t(2)=Y; %Change the value in the Y spot to the new one
            T.t(3)=Z; %Change the value in the Z spot to the new one
            Joint_Angles = obj.IkineRobot(T,[1,1,1,0,0,0],obj.Thetas);%Perform the inverse kinematics on matrix from above with the initial guess of the current joint angles
            for(i=1:1:obj.Num_Joints)%Loop through all the joints and move each one
                obj.MoveJoint(i,Joint_Angles(i));% Move the joints to the locations found by the ikine function
            end
            obj.Tool_Position=T.transl;
        end
        
        function T= MoveJoint(obj,Joint_Number,Angle)
            %INPUTS
            %      obj: This is automatically passed in when the function
            %       is called
            
            %      Servo_Number: The number of the servo that you want to
            %      move. This is consistent with the joint number that you
            %      want to move
            
            %      angle: The angle that you want to move the servo. This
            %      value MUST be in RAD
            
            %      obj.Joint_Direction: This is the minimum angle that the servo will
            %      turn. It is used to switch the direction of travel for
            %      the servo
            
            %      obj.Joint_Direction: This is the maximum angle that the servo will
            %      turn. It is used in conjuction with the obj.Joint_Direction to
            %      switch the direction of travel.
            %END INPUTS
            
            %NOTE: obj.Joint_Direction is either a 0 or a 1 and obj.Joint_Direction is the
            %opposite of this value. You need to figure out which set to
            %use to get the servo to turn in the direction you expect for
            %the given angle
            
            Checked_Angle=obj.CheckLimits(Joint_Number,Angle);%Check if the angle passed is in beyond the joint limit
            %We do this for all the conditions of hardware use or not, so
            %we just do it once and this makes the code easier to read.
            
            
            if(obj.Hardware_Used==1)
                Percentage=Arduino_Map(Checked_Angle,obj.Linked_Robot.qlim(Joint_Number,1),obj.Linked_Robot.qlim(Joint_Number,2),obj.Joint_Direction(Joint_Number,1),obj.Joint_Direction(Joint_Number,2));%Map the value from the angles to between 0 and 1
                writePosition(obj.Servos(Joint_Number),Percentage);%Write the percentage to the servo
                obj.Thetas(Joint_Number)=Checked_Angle;%Set the angles to the robot object that the GUI uses
                obj.PlotRobot();%Update the plots with the new thetas
                T=obj.FkineRobot();%Compute the end effector location with the current thetas that were just updated
            elseif(obj.Hardware_Used==2)%If hardware_Used is equal to 2, we are only using the hardware with no plotting of the gui
                Percentage=Arduino_Map(Checked_Angle,obj.Linked_Robot.qlim(Joint_Number,1),obj.Linked_Robot.qlim(Joint_Number,2),obj.Joint_Direction(Joint_Number,1),obj.Joint_Direction(Joint_Number,2));%Map the value from the angles to between 0 and 1
                writePosition(obj.Servos(Joint_Number),Percentage);%Write the percentage to the servo
            else%Otherwise, we are not using hardware at all and we are just using the GUI.
                obj.Thetas(Joint_Number)=Checked_Angle;%Set the GUI object thetas to the checked angle
                obj.PlotRobot();%Update the robot plot with the new angles
                T=obj.FkineRobot();%Compute the new end effector location
            end
        end
        
        function ConnectHardware(obj,ComPort,BoardType)%Function to connect the arduino to the robot
            %INPUTS
            %      obj: This is automatically passed in when the function
            %       is called
            
            %      ComPort: The port that the arduino should be on
            
            %      BoardType: The type of arduino used. ex. Uno,
            %      Mega2560,...
            %END INPUTS
            obj.Controller=arduino(ComPort,BoardType,'libraries','Servo');%Attempt to connect the arduino to matlab
            msgbox("Hardware Connected");%If the operation was successful, then display a message saying that
        end
        
        function AddServo(obj,Pin,Min_Pulse_Length,Max_Pulse_Length,Servo_Number,Replace)%Function to add a servo to the robot
            %INPUTS
            %      obj: This is automatically passed in when the function
            %       is called
            
            %      Pin: The pin that the servo is attached to. Must be in
            %      the form of 'D6', this is standard for what the arduino
            %      toolkit is expecting
            
            %      Min_Pulse_Length: The minimum pulse duration for the
            %      chosen servo. This is found with the datasheet
            
            %      Max_Pulse_Length: The maximum pulse duration for the
            %      chosen servo. This is found with the datasheet
            %END INPUTS
            if(strcmp(Servo_Number,'Last') && strcmp(Replace,'No'))% If the user wants to put the servo as the last joint and we are not replacing any joints
                obj.Servos=[obj.Servos, servo(obj.Controller,Pin,'MaxPulseDuration',Max_Pulse_Length,'MinPulseDuration',Min_Pulse_Length)];%Create a servo and append it to the end
                obj.Num_Servos=obj.Num_Servos+1;%Increment the number of servos
                obj.MoveJoint(obj.Num_Servos,0);% Move the servo to the zero angle position
            elseif(~strcmp(Replace,'No'))% If we are replacing a servo
                if(strcmp(Servo_Number,'Last'))%Check to see which place we are replacing, if its the last
                    Place=obj.Num_Servos;% Then place becomes the last element in the servos array
                else
                    Place=str2double(Servo_Number);% or, we replace a servo based on the users choice
                end
                obj.Servos(Place)=servo(obj.Controller,Pin,'MaxPulseDuration',Max_Pulse_Length,'MinPulseDuration',Min_Pulse_Length);%Create a servo and place in it the spot the user wanted
                obj.MoveJoint(Place,0);% Moe the joint to the zero angle
            elseif(~strcmp('Last',Servo_Number)&&strcmp(Replace,'No'))% If we are not replacing but the position is not the last, we are putting the joint somewhere in the middle
                S=[];%Create a dummy variable to hold the servos
                for(i=1:1:obj.Num_Servos)% Loop through the servos
                    if(i==str2double(Servo_Number))% Check to see if the current servo number matches the servo number the user passed in
                        S=[S,servo(obj.Controller,Pin,'MaxPulseDuration',Max_Pulse_Length,'MinPulseDuration',Min_Pulse_Length)];%If it does, then we will create a new servo and put it in the spot of i
                        obj.Num_Servos=obj.Num_Servos+1;%Increment the number of servos
                    end
                    S=[S,obj.Servos(i)];%Otherwise, we add the current servo configuration to the dummy variable
                    obj.MoveJoint(i,0);% Move the joint to the zero position
                end
                clear obj.Servos% Clear whats inside of the Servos variable and...
                obj.Servos=S;% Replace it with the dummy variable
            else
                %Do nothing
            end
        end
        
        function DetachHardware(obj)%Function to detach all hardware from the robot
            %INPUTS
            %       obj: This is automatically passed in when the function
            %       is called
            %END INPUTS
            obj.Controller=[]; %Clear the controller from the class
            obj.DetachServo(); % Detach the servos and since we didn't specify a number inside, we will clear all of them
            msgbox("All hardware has been detached from the robot");%Give a message box that tells the user that the hardware has been cleared
        end
        
        function DetachServo(obj,Servo_Number)% Function to detach servos on the robot
            S=[];%Creat a blank variable to store the new servos
            if(nargin == 1)%If no Servo number was given, we will delete all servos from the robot
                clear Obj.Servos;
                msgbox("All servos have been removed from the robot");%Tell the user that all the servos were deleted from the robot
            else%Otherwise a servo was specified
                for(i=1:1:obj.Num_Servos)%Loop through all the servos and add them to the blank variable. We do this as long as the counter isn't at the servo number specified by the user
                    if(i ~= Servo_Number)%If the counter is not the same as the servo number specified
                        S=[S,obj.Servos(i)];%Add the current servo into the temporary variable
                    end
                end
                obj.Num_Servos = obj.Num_Servos - 1;%We have deleted a servo, so we should update the robot to reflect that
                clear obj.Servos;%Clear the whole servos variable from the robot
                Message=['Servo ' ,char(Servo_Number),' removed from hardware.'];%Give the user a message saying that the servo they wanted to delete was deleted
                msgbox(Message);%Actually display that message
                obj.Servos=S;%Set the servos variable in the robot to the temporary variable, this will effectively replace the servos variable with the servos that werent specified
                clear S;%Clear the temporary variable
            end
        end
        
        function DeleteRobot(obj)%Function to delete the robot from memory
            obj.DetachHardware()%Call the function to detach all hardware
            clear obj.Num_Servos  %Clear all the variables associated with the robot
            clear obj.Num_Links
            clear obj.Thetas
            clear obj.Linked_Robot
            clear obj.Joint_Direction
            clear obj.Tool_Position
        end
    end
end