Figure 2-19: Reference frames bases on Denavit-Hartenberg representation. Using the schematic above, the so called DH parameters are determined. These are shown in the table below.
Table 1: DH parameters for robot arm. Indices for each degree of freedom are listed on the left. The values of each DOF are represented by the θ values which are unknown. The ‘joint offset’ is represented by d. This is zero in all cases for this robot because each joint is in the same plane. The lengths of each link, in meters, are listed in the column labeled a. The last column lists the angles between the x-axis of successive frames.
These parameters are used to define the transformation matrix between frames. This general form of this matrix is shown below.
Using this matrix, the following relationship defines the forward kinematic equation where each A matrix is written in terms of the corresponding parameters from the table above.
The individual equations for each element in terms of the joint angles are given in the appendix in addition to MATLAB code that can be used to compute the result for a given set of angles.
As can be seen by the resulting equations in the appendix, the inverse kinematic solution will be difficult to achieve. Each equation involves multiple coupled angles which make the problem difficult to solve analytically. A closed form solution for a simple five DOF robot such as this does exist, but in general the solution must be achieved numerically.
An attempt was made to use an artificial neural network to map the desired location and orientation to the corresponding joint angles. This was implemented using MATLAB’s Neural Network Toolbox. A two layer, feed-forward network, with 20 neurons was trained using the Levenberg-Marquardt method. This was done with a built-in GUI tool. The results of this experiment were not accurate. Without understanding neural network theory better, these results can’t be further interpreted. A link to the MATLAB code is listed in the appendix.
The mechanical arm is built from almost entirely pre-fabricated Tetrix aluminum components, two DC motors with gears, several small-scale servos, and is built to full-scale human arm size. Due to this, it takes a minimal amount of torque to cause vibration in or possibly warp the base components. This means that the mechanical arm cannot carry large amounts of weight. It is estimated that it can pick up slightly less than three pounds at full extension. However, the design is robust and allows large range of movement without detrimental effects on the structure, thus providing the possibility for a very human-like interaction with this arm.
Currently there are encoders attached to the two DC motors which control the ‘shoulder’ and ‘elbow’ vertical movements however they are not used. The encoders cause difficulty with the motors because the motors resist instantaneous position correction and they lock-up. Currently all position control is manual and user-operated through a RF wireless joystick controller.
The hand attached to the mechanical arm is designed to mirror a human hand. Currently it only has one DOF, its ability to open and close by moving the thumb. This however is sufficient for grasping and picking up objects. The movements are relatively slow so that they are somewhat more realistic. Additionally, if the servos speed is increased accuracy is lost.
In the following section we will provide a guide to installing and setting up RobotC. You must first start by downloading RobotC. You have two main options; you can purchase a copy from http://www.RobotC.net or you can find a beta for a future version for free on their forums (http://www.RobotC.net/forums). The specific thread changes over time, and has several times since we started the project. We got our specific version from this thread. If you search the forums, you will find another beta. These beta versions are good for 1 year, and we had very few problems with ours.
The first thing you will need to do is to install the USB driver found at the Mindstorms website.
Once you have installed RobotC the next step is to configure it and create a program. The PSP controller comes with a great demo program called "PSP-Nx-tank-drive.c" found at mindsensors.com.
This program provides an adequate base for additional code you wish to put in as well. To see the completed group code, see Appendix 2.
Next, we will need to configure the general aspects of RobotC. Since we are using a Mindstorms and Tetrix system, we will need to select the platform type indicating that. (See picture below)
Fig. 3-1RobotC Platform Selection Now we can move on to the setup of our motors. Fortunately, RobotC makes this incredibly easy. In the image below, you will see code that is at the very top of our program. This is the code that will define our servos’ and motors’ parameters.
Fig. 3-2 Automatically Generated RobotC Setup Code The first thing we need to do to automatically generate this code is to select the "Motors and Setup" option within the "Robot" menu option.
Fig. 3-3 Motors and Sensors Menu Option Within the "Motors and Sensors Setup" window that appears, go to the "Motors" tab. There, you will see a list that is automatically populated. Here you can choose names for your motors, we chose "motorD" and "motorE" for our base motors. You can also see the other motors which are used by the arm team. Notice in the picture below, that a check mark indicates that one of the motors is reversed. This is because RobotC, by default, correlates positive motor rotation in the clockwise direction and since the motors are rotated 180 degrees from each-other a normally ‘positive’ rotation will drive our robot in circles.
Fig. 3-4 Motors and Sensors Setup Window This completes the configuration for RobotC. You are now ready to program!
3.2.1 Technical Problems and Troubleshooting If you notice your axis is off by 90 degrees when you try to steer the base, you may need to re-select the reverse option, and re-compile and re-download the code.
We found that on some laptops, certain USB ports were "preferred." If you are having problems detecting the NXT Brick or Downloading the software, you may want to try switching ports.
3.2 NXT Brick Setup
The NXT Brick will act as a go between for all the different modules. It will take inputs from the Joystick, and allow control of the head and arm.
When you first connect the USB cable between the laptop and the NXT Brick, you may have to download the firmware. The firmware comes with RobotC.
First, within the "Robot" menu option, select "Download Firmware" (as seen below).
Fig. 3-5 Firmware Update Menu Option
Verify that the brick is recognized, the click the "F/W Download" button.
Fig. 3-6 Firmware Download Window 3.2.1 Technical Problems and Troubleshooting Sometimes the NXT brick will not load, and makes a clicking sound. There are a few possible causes ranging from corrupted files to giving it dirty looks. If and when this happens, you will need to re-download the firmware onto the NXT brick. Follow the instructions above under section 3.2.1 to reset it.
The purpose of this code is to not only to control the base, but the arm and head as well. This program will allow total control through the NXT brick, allowing the laptop to be specifically dedicated to the head. The buttons and analog sticks will control the arm, the base, and the directional buttons will be used to call emotion functions for the head team.
We will be primarily RobotC for the base and the arm, and use that to call functions for the head group. RobotC was developed by Carnegie Mellon University and is a subversion of the C programming language. It uses the same syntax as C but a does not have access to the same libraries, so the command availability is somewhat limited. There are specific libraries for some aftermarket parts, and libraries can be made to incorporate new parts for use with the NXT
We will cover code pertinent to specific modules below. All the code fit nicely into one file, calling from the same library functions.
3.3.1 Base code You will want to download two code files from the mindsensors website. You specifically want the PSP-Nx-tank-drive.c file and the PSP-Nx-lib.c files. The PSP-Nx-lib.c can be seen in Appendix 2. The PSP-Nx-tank-drive.c file can be replaced by the overall code
We will mainly include the code we used specifically for the base in this section, and will explain what is happening. The first thing in our program that we do with the controller is indicate what sensor port we will be using for our PSP controller. This is declared before the .lib files.
const tSensors SensorPort = S2;
a c square circle
The buttons are labeled as the following in the "PSP-Nx-Lib.c" file. You will see these values commonly referred to in the main program.
In the main program, we start by initializing our values as seen in the next code example.
int powerD = 0; //left motor
int powerE = 0; // right motor
// joystick buttons, init to 0
int d_left_X = 0; // getting x component
int d_left_Y = 0; // getting y component
Then, in the next part, we poll the port for button inputs. d_left_X and d_left_Y are polling the states of the left potentiometer. For more information on button mapping, see the PSP Controller Library Code in Appendix 2. The coordinates are being gathered into the d_left variables. Then we determine how much we should power the different motors by adding or subtracting them.
We noticed a problem with the direction our robot turned when moving in the reverse direction. Reverse left and reverse right were swapped. We solved this by implementing the IF statement seen below.
We found it necessary to scale back the actual power on the motors. We did this by taking the values after they had been calculated and dividing them by 3.
motor[motorD] = powerD/3;
motor[motorE] = powerE/3;
There were times that we found the joystick did not function as intended. We found that we either made a mistake in our code implementation or the program failed to upload onto the NXT Brick correctly (there is no error message). In order to accurately troubleshoot, we implemented the following diagnostic code:
nxtDisplayTextLine(1,"left X val: %d", d_left_X);
nxtDisplayTextLine(2,"left Y val: %d", d_left_Y); nxtDisplayTextLine(3,"motorD: %d", powerD);
nxtDisplayTextLine(4,"motorE: %d", powerE);
This code displays the X and Y values of the left potentiometers on the NXT display, the motor power being calculated, and the rev of the software. The numbers indicate line numbers, the quotations what the comment is, the %d displays the value of the variable to the right.