6 DOF Robot Arm




This tutorial deals with the theory and implementation of the control of a 6 degree of freedom robot arm. The main idea is to use a C# form to obtain inputs from the user and send those inputs to MATLAB to perform trajectory calculations. The commands are then sent in regular intervals through Bluetooth to Arduino which then sends individual axis commands to a PWM driver via I2C.


As it turns out, in this system most of the components could be bought from an electronics store. The main components are listed below.

  1. Arduino Pro Mini (Microcontroller)
  2. PCA9685 (16-Channel PWM Driver)
  3. HC-06 (Bluetooh Module)
  4. Variable DC step-down (~6V at 6A)
  5. UF5404 (High current diodes)
  6. 6x hobby servos (High torque metal geared)
  7. Robot Arm Chassis/Claw

Note: The main issue that must be dealt with is the potential high current draw at 6V. Most 6V supplies cannot supply such large peak current draws and therefore voltage regulators will be used to drop a 12V DC source to 6V. In my case, I was able to obtain two 3A variable voltage regulators and connect the outputs together using diodes to isolate the supplies from each other. The idea is that if the current draw is too large, the other regulator will share the load. 6A total was chosen since each servo was budgeted approximately 1A (this is a rule of thumb). The reason why the variable voltage aspect is important is due to possibility that the two diodes in OR configuration (diode OR gate) will have different voltage drops.


Below is the schematic of the hardware connections between the Arduino, HC-06, voltage regulators and PCA8685 driver. The connections between the different components can be done directly as each of the boards have necessary protection circuitry and logic level shifts. Note that the servos are not shown


This section detail the implementation of how communication is done from C# to Arduino via Bluetooth and C# to MATLAB.

C# to Arduino (Bluetooth)

In order to use Bluetooth communication, you must first pair your HC-06 or other Bluetooth device to your computer. The computer will show it has a COM port device which is the exact same way an Arduino will show up. The relevant default settings for the HC-06 are shown below:

  1. Baud Rate: 9600 (8N1)
  2. PIN: 1234
  3. Name: HC-06

8-N-1 is a common notation that specifies the format of the data coming in and going out. This means that  8 bits with No parity and 1 stop bit are being sent in discrete packets. A baud rate of 9600 is indicative of how fast the transfer occurs 9600 meaning 9600 bits per second. This creates a problem, as 8 bits necessarily implies that only a number between 0-255 can be sent in one packet. However, if we are controlling angles of servos, then only numbers between 0-180 representing the rotation in degrees is necessary.


The idea is that a start byte indicating that a command is beginning is sent first to the Arduino. The Arduino then listens for the information until it sees another start byte in which case it realizes another command is being sent. In our case, there are 6 servos, meaning 6 numbers from 0-180 are sent in each command. The data sent to Arduino is summarized in a list below.

  1. Start byte (255)
  2. Axis command 0 (0-180)
  3. Axis command 1 (0-180)
  4. Axis command 2 (0-180)
  5. Axis command 3 (0-180)
  6. Axis command 4 (0-180)
  7. Axis command 5 (0-180)

Assuming that you know the basics of C# using the Toolbox for Forms, just add a "SerialPort" to the form and configure it to 9600 8N1. Note that if you used AT commands with the HC-06 you can change the baud rate. In my case I used 19200 baud rate. The settings for reference are given below.

Then the knitty gritty around displaying the COM ports and selecting them is needed, the code in the form of a Visual Studio solution will be provided at the end of the tutorial :). The portion of the code to look for is when the transmission begins in the serial port. In theory, you can send the bytes in any order, and u can also send any number of bytes during one command. The snippet is given below so you know what to look for. Following the C# Master Code, the Arduino Slave code for receiving commands is also given. The main idea is that the commands from C# are stored in the Arduino after these key executions are complete.

C# Master Code

            byte[] TxBytes = new byte[7]; // create vector of new bytes
            // send all bytes
            TxBytes[0] = Convert.ToByte(255);
            TxBytes[1] = Convert.ToByte(vScrollBar1.Value);
            TxBytes[2] = Convert.ToByte(vScrollBar2.Value);
            TxBytes[3] = Convert.ToByte(vScrollBar3.Value);
            TxBytes[4] = Convert.ToByte(vScrollBar4.Value);
            TxBytes[5] = Convert.ToByte(vScrollBar5.Value);
            TxBytes[6] = Convert.ToByte(vScrollBar6.Value);

            if (serCOM.IsOpen == true)
                // write all the bytes
                for (int i = 0; i < 7; i++)
                    serCOM.Write(TxBytes, i, 1);


Arduino Slave Code

void loop() {

  // receive command from the serial
  if(Serial.available() >= 7) // wait for sufficient data
      byte inByte = Serial.read(); // get incoming byte

      if( inByte == 255 ) // if it is a start byte read all the data
      for(int i = 0; i < 6; i++)
        pos[i] = Serial.read();


Note that once the Arduino is hooked up to the HC-06, transmission of data can already happen. As long as the baud rate of the HC-06 and the Arduino are the same the communication will work. It is a good idea to just test the HC-06 and Arduino communication at this point.

Trajectory Generation

The whole point of offloading calculations to MATLAB is that there are many easy to use functions that make MATLAB programming a lot easier than writing functions from scratch and working with different types of arrays or lists. That being said, it is of course possible to implement these calculations without MATLAB if you either don't have it or just don't want to use it. 

The idea behind trajectory generation is that you know a path that you wish to move along as well as the speed that you wish to move along it. The question is, what are commands that need to be sent in order for the point of interest, often called an "end-effector" such as a claw or milling bit move along that path at a specified velocity profile. In this case, we will do a simple line between two points, at a constant velocity.

My tutorial here on trajectory generation covers lines and general curves. Note that in these tutorials, a method in which velocity ramps upwards is used. This is necessary in precision systems, however servo commands are very low resolution and velocity "ramping" has no real benefit.

Assuming that the trajectory is now planned (it should come out as a vector of X,Y components) we now work on deriving the relationships. The general approach is to linearize the system in two variables and then invert the system explicitly and incrementally to derive the path. Suppose we have a robot with 3 linkages, and 3 motors placed at the origin, \(x_1,x_2\).

The equations describing \(x_1,x_2\) are given below. Let us suppose that \(x_2\) is the end effector for now in order to avoid a redundant degree of freedom.

$$ x_1 = (L_1 cos(\theta_1),L_1 sin(\theta_1)) $$

$$ x_2 = (L_2 cos(\theta_1 + \theta_2),L_2 sin(\theta_1 + \theta_2)) + x_1 $$

Now linearize \(x_2\)

$$  \Delta x_2 \approx \frac{dx_2}{d\theta_1} \Delta \theta_1  + \frac{dx_2}{d\theta_2} \Delta \theta_2 $$


$$ \Delta x_2 = x_{2,n} - x_{2,n-1} $$

$$ x_{2,0} = x_2(\theta_{1,0} ,\theta_{2,0}) $$

Finally we can write the matrix equation to be solved for each increment

$$ \begin{bmatrix}  u_x & v_x \\  u_y & v_y  \end{bmatrix}  \begin{bmatrix}  \Delta \theta_1  \\  \Delta \theta_2   \end{bmatrix}  = \begin{bmatrix}  \Delta x_{2,x}  \\  \Delta x_{2,y}   \end{bmatrix} $$  

The critical MATLAB code to generate a path between \(x_0\) and \(x_1\) at some velocity is shown below.

Matlab Code

x = @(t1,t2) [ L2*cos(t1+t2)+L1*cos(t1); L2*sin(t1+t2)+L1*sin(t1)];

% Suppose the plot was a line from x0 to 10,10 split into 100 pieces
x0 = x(th1,th2);

x1 = [px,py]';

T = norm(x1-x0)/vel; % time to complete motion

N = round(T/(1/50)); % Divide time by the update time

posx = [linspace(x0(1),x1(1),N)' linspace(x0(2),x1(2),N)'];

inposx = diff(posx);

n = length(inposx);
% store all angles over time
tVector = zeros(n+1,2);

tVector(1,:) = [th1,th2];

for i=1:n
    % solve for the incremental angle
    A = [ dth1(tVector(i,1),tVector(i,2)) dth2(tVector(i,1),tVector(i,2))];
    incth = linsolve(A,inposx(i,:)');
    % Add incremental angle to get absolute angle
    tVector(i+1,:) = tVector(i,:) + incth';

C# Code for MATLAB

First I recommend that you look at MATLAB's documentation on calling functions from C#, you can copy paste the code and it will work provided you've added "Matlab Application Type Library" via the References tab. I found that one line was buggy which was the line specifying the directory, basically an extra single quote is added to the address shown below.

matlab.Execute(@"cd 'C:\Users\Owen\Documents\MATLAB\ROBOT ARM'");

The critical C# code that gets an array from MATLAB and stores it to a C# variable is shown below.

                    object result = null;
                    //// Call the MATLAB function myfunc
                    matlab.Feval("gentraj", 1, out result, Convert.ToDouble(txtServo2.Text), Convert.ToDouble(txtServo3.Text), Convert.ToDouble(px), Convert.ToDouble(py), Convert.ToDouble(txtSpeed.Text)); // Convert.ToDouble(txtSpeed.Text)
                    var res = (result as object[]).Select(x => (double[,])x).ToArray();
                    object t_angArray = res.GetValue(0);
                    angArray = (double[,])t_angArray;


Now that you have had a basic run through of the methods used, hopefully you can examine the code and figure out what is going on. Below is the source code for each of the components. It is assumed that you have installed the PCA9685 library. Adafruit has very good documentation and instructions online on how you can get the driver working.

C# Visual Studio SLN

Arduino Code

MATLAB Functions