Mech - User contributions [en]

High Speed Vision System and Object Tracking

2010-06-05T21:48:28Z

ClaraSmart: /* For Tracking Objects in 2D */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.

Once downloaded, to open the gui in matlab, go to the toolbox directory then type 'calib_gui' in the command window. (Then select standard memory).

==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 30mm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

Error optimization completed by adjusting the values of winty and wintx for various images.

Calibration results after optimization (with uncertainties):

Focal Length: fc = [ 1219.49486 1227.09415 ] ± [ 1.98092 1.95916 ]

Principal point: cc = [ 502.28854 524.25824 ] ± [ 1.26522 1.07298 ]

Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes = 90.00000 ± 0.00000 degrees

Distortion: kc = [ -0.39504 0.36943 -0.00018 -0.00105 0.00000 ] ± [ 0.00307 0.01033 0.00016 0.00021 0.00000 ]

Pixel error: err = [ 0.27578 0.25400 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

[[Image:CalibImages.jpg|thumb|200px|Mosaic of Calibration Images|left]]
[[Image:ErrorPlot.jpg|thumb|400px|Error Plot following optimization|center]]

High Speed Vision System and Object Tracking

2010-06-05T21:46:54Z

ClaraSmart: /* Example Intrinsic Parameters (LIMS Lab, Winter 2010) */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.

Once downloaded, to open the gui in matlab, go to the toolbox directory then type 'calib_gui' in the command window. (Then select standard memory).

==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

Error optimization completed by adjusting the values of winty and wintx for various images.

Calibration results after optimization (with uncertainties):

Focal Length: fc = [ 1219.49486 1227.09415 ] ± [ 1.98092 1.95916 ]

Principal point: cc = [ 502.28854 524.25824 ] ± [ 1.26522 1.07298 ]

Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes = 90.00000 ± 0.00000 degrees

Distortion: kc = [ -0.39504 0.36943 -0.00018 -0.00105 0.00000 ] ± [ 0.00307 0.01033 0.00016 0.00021 0.00000 ]

Pixel error: err = [ 0.27578 0.25400 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

[[Image:CalibImages.jpg|thumb|200px|Mosaic of Calibration Images|left]]
[[Image:ErrorPlot.jpg|thumb|400px|Error Plot following optimization|center]]

High Speed Vision System and Object Tracking

2010-06-05T21:45:28Z

ClaraSmart: /* Example Intrinsic Parameters (LIMS Lab, Winter 2010) */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.

Once downloaded, to open the gui in matlab, go to the toolbox directory then type 'calib_gui' in the command window. (Then select standard memory).

==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

Error optimization completed by adjusting the values of winty and wintx for various images.

Calibration results after optimization (with uncertainties):

Focal Length: fc = [ 1219.49486 1227.09415 ] ± [ 1.98092 1.95916 ]
Principal point: cc = [ 502.28854 524.25824 ] ± [ 1.26522 1.07298 ]
Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes = 90.00000 ± 0.00000 degrees
Distortion: kc = [ -0.39504 0.36943 -0.00018 -0.00105 0.00000 ] ± [ 0.00307 0.01033 0.00016 0.00021 0.00000 ]
Pixel error: err = [ 0.27578 0.25400 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

[[Image:CalibImages.jpg|thumb|200px|Mosaic of Calibration Images|left]]
[[Image:ErrorPlot.jpg|thumb|200px|Error Plot following optimization|right]]

High Speed Vision System and Object Tracking

2010-06-05T21:44:26Z

ClaraSmart: /* Example Intrinsic Parameters (LIMS Lab, Winter 2010) */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.

Once downloaded, to open the gui in matlab, go to the toolbox directory then type 'calib_gui' in the command window. (Then select standard memory).

==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

Error optimization completed by adjusting the values of winty and wintx for various images.

Calibration results after optimization (with uncertainties):

Focal Length: fc = [ 1219.49486 1227.09415 ] ± [ 1.98092 1.95916 ]
Principal point: cc = [ 502.28854 524.25824 ] ± [ 1.26522 1.07298 ]
Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes = 90.00000 ± 0.00000 degrees
Distortion: kc = [ -0.39504 0.36943 -0.00018 -0.00105 0.00000 ] ± [ 0.00307 0.01033 0.00016 0.00021 0.00000 ]
Pixel error: err = [ 0.27578 0.25400 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]
[[Image:CalibImages.jpg|thumb|500px|Mosaic of Calibration Images|left]]
[[Image:ErrorPlot.jpgthumb|500px|Error Plot following optimization|right]]

File:ErrorPlot.jpg

2010-06-05T21:42:10Z

ClaraSmart: Error Plot following calibration and optimization.

Error Plot following calibration and optimization.

File:CalibImages.jpg

2010-06-05T21:41:18Z

ClaraSmart: uploaded a new version of "Image:CalibImages.jpg": Mosaic of the images used for calibration.

File:CalibImages.jpg

2010-06-05T21:38:43Z

ClaraSmart:

High Speed Vision System and Object Tracking

2010-06-05T21:38:13Z

ClaraSmart: /* Example Intrinsic Parameters (LIMS Lab, Winter 2010) */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.

Once downloaded, to open the gui in matlab, go to the toolbox directory then type 'calib_gui' in the command window. (Then select standard memory).

==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

Error optimization completed by adjusting the values of winty and wintx for various images.

Calibration results after optimization (with uncertainties):

Focal Length: fc = [ 1219.49486 1227.09415 ] ± [ 1.98092 1.95916 ]
Principal point: cc = [ 502.28854 524.25824 ] ± [ 1.26522 1.07298 ]
Skew: alpha_c = [ 0.00000 ] ± [ 0.00000 ] => angle of pixel axes = 90.00000 ± 0.00000 degrees
Distortion: kc = [ -0.39504 0.36943 -0.00018 -0.00105 0.00000 ] ± [ 0.00307 0.01033 0.00016 0.00021 0.00000 ]
Pixel error: err = [ 0.27578 0.25400 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

[[Image:CalibImages.jpg]]
[[Image:ErrorPlot.jpg]]

High Speed Vision System and Object Tracking

2010-06-05T21:30:21Z

ClaraSmart: /* Calibrating the High Speed Camera */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.

Once downloaded, to open the gui in matlab, go to the toolbox directory then type 'calib_gui' in the command window. (Then select standard memory).

==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

High Speed Vision System and Object Tracking

2010-06-05T21:14:06Z

ClaraSmart: /* Example Intrinsic Parameters (LIMS Lab, Winter 2010) */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

Number of Images = 15

High Speed Vision System and Object Tracking

2010-06-05T21:02:55Z

ClaraSmart: /* Error Minimization of Calibration Coefficients */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

==Example Intrinsic Parameters (LIMS Lab, Winter 2010) ==

The below example was completed for the high speed vision system in the LIMS lab during Winter Quarter 2010.

High Speed Vision System and Object Tracking

2010-06-05T21:01:34Z

ClaraSmart: /* Error Minimization of Calibration Coefficients */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

Following the computation of the intrinsic parameters, the relative error can be analyzed using the calibration toolbox. To see the error in a graphical form click 'Analyze Error' in the main gui. This plot will show the error in pixel units. The different point colors represent different images used for calibration. To begin to optimize this error, click on an outlier point. The image number and point of this point will be displayed in the matlab command window. Click outliers of the same color, is there a specific image that has many errors?

You can also view the projected calibration points on a specific image by clicking 'Reproject on Image' and at the prompt enter the number of the error prone image. This will allow you to see the points which are creating the most error. If the calibration points are significantly off the corners of the grid, it may be a good idea to use 'Add/Suppress Images' to remove then recalibrate the image. (You can also click all corners manually)

However, if the entire image is not error prone, it is possible to minimize the error by changing the dimensions of the "search box" for which the calibration algorithm looks for a corner. This is accomplished by changing the values of wintx and winty, the default values during calibration are set at 5. To do this click 'Recomp Corners' in the calibration gui, then at the prompt for wintx and winty enter a different (typically larger value, ie 8). Then at the prompt for the numbers of the images for which to recompute the corners, enter the numbers of the images with the error outliers as determined above.

Repeat this process until the error is within reason. (Typically 3-4 pixels, but dependent on resolution, distance from object, pixel size and application)

Further information and examples can be found at [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html, the first calibration example].

High Speed Vision System and Object Tracking

2010-06-05T20:43:04Z

ClaraSmart: /* Calculating the Position of an Object on a Plane: Using Matlab */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist_pix *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist_pix is the distance from the focal point to the object in pixels. (measure distance from camera to object using meter stick then convert to pixels by determining the number of pixels per cm in calibration image at that distance)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

High Speed Vision System and Object Tracking

2010-06-05T20:40:15Z

ClaraSmart: /* Post-Processing of Data */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist is the distance from the focal point to the object in world units. (measure distance from camera to object using meter stick etc)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

==Error Minimization of Calibration Coefficients ==

High Speed Vision System and Object Tracking

2010-06-05T20:38:48Z

ClaraSmart: /* Calculating the Position of an Object on a Plane: Using Matlab */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1) - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist is the distance from the focal point to the object in world units. (measure distance from camera to object using meter stick etc)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

High Speed Vision System and Object Tracking

2010-06-05T20:37:10Z

ClaraSmart: /* Post-Processing of Data */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Calculating the Position of an Object on a Plane: Using Matlab==

Similar to the above discussion, a point in 3D space can be determined (in world coordinates) based on the intrinsic and extrinsic parameters from calibration with a MatLab function.
(It is assumed the Z value is a plane and is a constant known parameter)

[World_x, World_y] = Transpose(Rc_ext) * (zdist *(normalize([pix_x, pix_y], fc, cc, kc, alpha_a), 1] - Tc_ext)

Where:
Rc_Ext and Tc_ext are parameters from the extrinsic calibration
zdist is the distance from the focal point to the object in world units. (measure distance from camera to object using meter stick etc)
pix_x and pix_y are the camera pixel coordinates corresponding to the desired world point
fc, cc, kc and alpha_a are parameters from the intrinsic camera calibration

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

High Speed Vision System and Object Tracking

2010-06-05T20:23:48Z

ClaraSmart: /* Calculating the Position of an Object in 3D */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D: Mathematical Theory==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

High Speed Vision System and Object Tracking

2010-06-05T20:22:18Z

ClaraSmart: /* For Tracking Objects in 2D */

__TOC__
=Calibrating the High Speed Camera=
[[image:Distorted_calibration_grid.jpg|thumb|200px|'''Figure 1:''' Calibration grid with distortion.|right]]
Before data can be collected from the HSV system, it is critical that the high speed camera be properly calibrated. In order to obtain accurate data, there is a series of intrinsic and extrinsic parameters that need to be taken into account. Intrinsic parameters include image distortion due to the camera itself, as shown in Figure 1. Extrinsic parameters account for any factors that are external to the camera. These include the orientation of the camera relative to the calibration grid as well as any scalar and translational factors.

In order to calibrate the camera, download the [http://www.vision.caltech.edu/bouguetj/calib_doc/ Camera Calibration Toolbox for Matlab]. This resource includes detailed descriptions of how to use the various features of the toolbox as well as descriptions of the calibration parameters.
==For Tracking Objects in 2D==
To calibrate the camera to track objects in a plane, first create a calibration grid. The Calibration Toolbox includes a calibration template of black and white squares of side length = 3cm. Print it off and mount it on a sheet of aluminum, foam core board or PVC to create a stiff backing. This grid must be as flat and uniform as possible to obtain an accurate calibration.

===Intrinsic Parameters===
Following the model of the [http://www.vision.caltech.edu/bouguetj/calib_doc/ first calibration example] on the Calibration Toolbox website, use the high speed camera to capture 10-20 images, holding the calibration grid at various orientations relative to the camera. 1024x1024 images can be obtained using the "Moments" program. The source code can be checked out [http://code.google.com/p/lims-hsv-system/ here].

[[image:Undistorted_calibration_grid.jpg|thumb|200px|'''Figure 2:''' Calibration grid undistorted.|right]]
Calibration Tips:
* One of the images must have the calibration grid in the tracking plane. This image is necessary for calculating the extrinsic parameters and will also be used for defining your origin.
* The images must be saved in the same directory as the Calibration Toolbox.
* Images must be saved under the same name, followed by the image number. (image1.jpg, image2.jpg...)
* The first calibration example on the Calibration Toolbox website uses 20 images to calibrate the camera. I have been using 12-15 images because sometimes the program is incapable of optimizing the calibration parameters if there are too many constraints.
* If a matlab error is generated before the calibration is complete, calibration will be lost. To prevent this, calibrate a set of images, then add groups of images to this set. To do this use the Add/Suppress Image feature. (Described in the 'first example' link above)

The Calibration Toolbox can also be use to compute the undistorted images as shown in Figure 2.

After entering all the calibration images, the Calibration Toolbox will calculate the intrinsic parameters, including focal length, principal point, and undistortion coefficients. There is a complete description of the calibration parameters [http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html here].

===Extrinsic Parameters===
In addition, the Toolbox will calculate rotation matrices for each image (saved as Rc_1, Rc_2...). These matrices reflect the orientation of the camera relative to the calibration grid for each image.
[[image:2D_imaging.jpg|thumb|500px|'''Figure 3:''' Projection of calibration grid onto the image plane.|center]]

Figure 3 is an illustration of the [http://en.wikipedia.org/wiki/Pinhole_camera_model pinhole camera model] that is used for camera calibration. Since the coordinates of an object captured by the camera are reported in terms of pixels (camera frame), these data have to be converted to metric units (world frame).

In order to solve for the extrinsic parameters, first use the 'normalize' function provided by the calibration toolbox:
[xn] = normalize(x_1, fc, cc, kc, alpha_c);
where x_1 is a matrix containing the coordinates of the extracted grid corners in pixels for image1.
The normalize function will apply any intrinsic parameters and return the (x,y) point coordinates free of lens distortion. These points will be dimensionless as they will all be divided by the focal length of the camera. Next, change the [2xn] matrix of points in (x,y) to a [3xn] matrix of points in (x,y,z) by adding a row of all ones.
xn should now look something like this:

<math>\mathbf{xn} = \begin{bmatrix}

x_c1/z_c & x_c2/z_c &... & x_cn/z_c \\
y_c1/z_c & y_c2/z_c &... & y_cn/z_c \\
z_c/z_c & z_c/z_c &... & z_c/z_c \end{bmatrix}</math>
Where x_c, y_c, z_c, denote coordinates in the camera frame. z_c = focal length of the camera as calculated by the Calibration Toolbox.

Then, apply the rotation matrix, Rc_1:
[xn] = Rc_1*x_n;
We now have a matrix of dimensionless coordinates describing the location of the grid corners after accounting for distortion and camera orientation. What remains is to apply scalar and translational factors to convert these coordinates to the world frame. To do so, we have the following equations:
: <math>X_1(1,1) = S1*xn(1,1) + T1 \, </math>
: <math>X_1(2,1) = S2*xn(2,1) + T2 \, </math>
Where:
: <math>S = (focallength)*(numberofmillimeters/pixel) \, </math>
X_1 is the matrix containing the real world coordinates of the grid corners and is provided by the Calibration Toolbox.
Cast these equations into a matrix equation of the form:
: <math>Ax = b \, </math>
Where:

<math>\mathbf{A} = \begin{bmatrix}
xn(1,1) & 1 & 0 & 0 \\
0 & 0 & xn(2,1) & 1 \\
xn(1,2) & 1 & 0 & 0 \\
0 & 0 & xn(2,2) & 1 \\
. & . & . & . \\
. & . & . & . \\
. & . & . & . \\
xn(1,n) & 1 & 0 & 0 \\
0 & 0 & xn(2,n) & 1 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
S_1\\
T_1\\
S_2\\
T_2\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
X_1(1,1)\\
X_1(2,1)\\
X_1(1,2)\\
X_1(2,2)\\
.\\
.\\
.\\
X_1(2,n)\\
X_1(2,n)\end{bmatrix}</math>

Finally, use the Matlab backslash command to compute the least squares approximation for the parameters S1, S2, T1, T1:
x = A\b

Now that all the parameters have been calculated, percentage error can be calculated, by applying the entire transformation process to the x_1 matrix and comparing the results to the X_1 matrix. If carefully executed, this calibration method can yield percent errors as low as 0.01%.

==For Tracking Objects in 3D==
In order to obtain the coordinates of an object in three dimensions, a mirror can be used in order to create a "virtual camera," as shown in Figure 4.
[[image:3D_imaging.jpg|thumb|600px|'''Figure 4:''' Three dimensional imaging with mirror at 45 degrees.|center]]
Calibrating the camera to work in three dimensions is very similar to the two dimensional calibration. The main difference is the calibration grid itself. Instead of using a single flat plane for the calibration grid, an additional grid has to be constructed, consisting of two planes at 90 degrees as shown in the figure. This grid must be visible by both the "real" and "virtual" cameras. The purpose of this is to create a common origin. Having a common origin for both cameras will become critical in calculating an object's position in three dimensions.

Follow the steps for the 2D calibration and complete a separate calibration for each camera. The two sets of intrinsic parameters that are obtained should be identical in theory. In practice, they are likely to differ slightly.

Once the real world coordinates of an object in the XZ and YZ planes are obtained, the 3D position of the object still has to be obtained. The raw data is inaccurate because it represents the projection of the object's position onto the calibration plane. For the methodology and code required to execute these calculations, refer to the section entitled "Calculating the Position of an Object in 3D."

=Using the High Speed Vision System=
[[image:VPOD_Setup.jpg|thumb|300px|'''Figure 5:''' VPOD with high speed vision 3D setup.|right]]
Figure 5 shows the current setup for the high speed vision system. The high speed camera is mounted on a tripod. The three lights surrounding the setup are necessary due to the high frequency with which the images are taken. The resulting camera shutter time is very small, so it is important to provide lots of light. The high frequency performance also has to be taken into consideration when processing the images. Processing one thousand 1024x1024 pixel images in real time proves impossible as well as unnecessary. The accompanying C program works by selection one or several regions of interest to focus on. The program applies a threshold to these regions. It then calculates an area centroid based off of the number of white pixels and re-centers the region of interest by locating the boundaries between the white and black pixels. The complete code for this program can be found [http://code.google.com/p/lims-hsv-system/ here].

I will include a brief description of some of the important constants, which can be found in the header file 'constants.h'

EXPOSURE & FRAME_TIME: These parameters control number of images to be taken in one second, and are measured in microseconds. Frame time is the amount of time required to take on image. Exposure time is the amount of time the camera shutter is open, and should always be less than the frame time. A ratio of 5:2, FRAME:EXPOSURE seems to work well. To run the camera at 1000images/s for example, EXPOSURE and FRAME_TIME should be set to 400 and 1000, respectively.

SEQ & SEQ_LEN: For multiple regions of interest, the sequence describes the order in which the regions of interest are addressed. The current setup has the program cycling through the regions in order, so for 1000Hz with two regions, each region would have a frequency for 500images/s, and the camera would alternate between the two regions. For such a setup, SEQ would be set to {ROI_0, ROI_1} and SEQ_LEN set to 2.

INITIAL_BLOB_XMIN, INITIAL_BLOB_YMIN, INITIAL_BLOB_WIDTH, INITIAL_BLOB_HEIGHT: These parameters describe the initial position and size of the blob surrounding your object of interest. These should be set such that the blob surrounds the initial position of the object.

THRESHOLD: The threshold is the value (0-255) which is the cutoff value for the black/white boundary. The threshold will have to be adjusted depending on ambient light and the exposure time. It should be set such that the object of interest or fiduciary marker is in the white realm while everything in the background becomes black.

DISPLAY_TRACKING: This quantity should always be set to one or zero. When equal to one, it displays the region or regions of interest. This is convenient for setting the threshold and positioning the blobs. While taking data, however, this should be disabled in order to speed up the program.

DTIME: This value is the number of seconds of desired data collection. In order to collect data, run the program, wait for it to initialize, and then press 'd'. 'q' can be pressed at any time to exit the program.

=Post-Processing of Data=
==Calculating the Position of an Object in 3D==
By comparing the data captured by the "real" camera and the "virtual" camera, it is possible to triangulate the three dimensional position of an object. The calibration procedures discussed earlier provide the real world (x,y) coordinates of any object, projected onto the calibration plane. By determining the fixed location of the two cameras, it is possible to calculate two position vectors and then compute a least squares approximation for their intersection.

The Calibration Toolbox provides the principal point for each camera measured in pixels. The principal point is defined as the (x,y) position where the principal ray (or optical ray) intersects the image plane. This location is marked by a blue dot in Figure 4. The principal point can be converted to metric by applying the intrinsic and extrinsic parameters discussed in the section "Calibrating the High Speed Camera."

From here, the distance from the cameras to the origin can be determined from the relationship:
: <math>distance = (focallength)*(numberofmillimeters/pixel) \, </math>

Once this distance is calculated, the parallel equations can be obtained for a line in 3D with 2 known points:
: <math>(x-x_1)/(x_2-x_1)=(y-y_1)/(y_2-y_1)=(z-z_1)/(z_2-z_1) \, </math>
Which can be rearranged in order to obtain 3 relationships:
: <math>x*(y_2-y_1)+y*(x_1-x_2) = x_1*(y_2-y_1) + y_1*(x_1-x_2) \, </math>
: <math>x*(z_2-z_1)+z*(x_1-x_2) = x_1*(z_2-z_1) + z_1*(x_1-x_2) \, </math>
: <math>y*(z_2-z_1)+z*(y_1-y_2) = y_1*(z_2-z_1) + y_1*(y_1-y_2) \, </math>
These equations can be cast into matrix form:

<math>\mathbf{A} = \begin{bmatrix}
y_2-y_1 & x_1-x_2 & 0 \\
z_2-z_1 & 0 & x_1-x_2 \\
0 & z_2-z_1 & y_1-y_2 \end{bmatrix}</math>
<math>\mathbf{x} = \begin{bmatrix}
x\\
y\\
z\end{bmatrix}</math>
<math>\mathbf{b} = \begin{bmatrix}
x_1*(y_2-y_1) + y_1*(x_1-x_2)\\
x_1*(z_2-z_1) + z_1*(x_1-x_2)\\
y_1*(z_2-z_1) + y_1*(y_1-y_2)\end{bmatrix}</math>

There should be one set of equations for each vector, for a total of six equations. A should be a [6x3] matrix, and b should be a vector of length 6. Computing A\b in Matlab will yield the least squares approximation for the intersection of the two vectors.

<nowiki>% The function "calc_coords" takes as inputs, the raw data for vision in
% the XZ and XY planes.

% CC_xz and CC_yz are vectors of size [2x1] containing the principal points
% for each vision plane. Coordinates for principal point should be computed beforehand and reported in
% metric.

% FC_xz and FC_yz are vectors of size [2x1] containing the two focal
% lengths of the lens for each vision plane. These are obtained from the
% Calibration Toolbox and should be left in pixels.

% The function returns a matrix of size [nx3] containing the least squares
% approximation of the (x,y,z) coordinates based off of the vectors from
% the two cameras.

% NOTES:
% - All computations are done in metric. (Based off of calibration grid)
% - The XZ vision plane = "Real Camera"
% - The YZ vision plane = "Virtual Camera"

function XYZ = calc_coords(XZ, YZ, CC_xz, CC_yz, FC_xz, FC_yz);
matSize_xz = size(XZ);
num_datapoints_xz = matSize_xz(1,1);
matSize_yz = size(YZ);
num_datapoints_yz = matSize_yz(1,1);
num_datapoints = min(num_datapoints_xz, num_datapoints_yz);
XYZ = zeros(num_datapoints-1, 4); % Initialize destination matrix
conv_xz = 5.40; % Conversion factor (number of pixels / mm) for vision in XZ
conv_yz = 3.70; % Conversion factor (number of pixels / mm) for vision in YZ
dist_origin_xz = [(FC_xz(1,1) + FC_xz(2,1)) / 2] / [conv_xz]; % Taking an average of the two focal lengths,
dist_origin_yz = [(FC_yz(1,1) + FC_yz(2,1)) / 2] / [conv_yz]; % Multiplying by conversion factor to get
% the distance to the calibration grid in metric.
prinP_xz = [CC_xz(1,1); -dist_origin_xz; CC_xz(2,1)]; % [3X1] vector containing the coordinates of the principal points
prinP_yz = [dist_origin_yz; -CC_yz(1,1); CC_yz(2,1)]; % for the "Real" and "Virtual" Cameras.
for i = [1 : 1 : num_datapoints - 1]
planeP_xz = [XZ(i,3); 0; XZ(i,4)]; % [3x1] vector containing the points in the plane of the calibration grid
planeP_yz = [0; -(YZ(i+1,3)+YZ(i,3))/2; (YZ(i+1,4)+YZ(i,4))/2];
x1_xz = prinP_xz(1,1);
y1_xz = prinP_xz(2,1);
z1_xz = prinP_xz(3,1);
x1_yz = prinP_yz(1,1);
y1_yz = prinP_yz(2,1);
z1_yz = prinP_yz(3,1);
x2_xz = planeP_xz(1,1);
y2_xz = planeP_xz(2,1);
z2_xz = planeP_xz(3,1);
x2_yz = planeP_yz(1,1);
y2_yz = planeP_yz(2,1);
z2_yz = planeP_yz(3,1);
% Set up matrices to solve matrix equation Ac = b, where c = [x, y, z]
A_xz = [ (y2_xz - y1_xz), (x1_xz - x2_xz), 0;
(z2_xz - z1_xz), 0, (x1_xz - x2_xz);
0, (z2_xz - z1_xz), (y1_xz-y2_xz) ];
b_xz = [ x1_xz*(y2_xz - y1_xz) + y1_xz*(x1_xz - x2_xz);
x1_xz*(z2_xz - z1_xz) + z1_xz*(x1_xz - x2_xz);
y1_xz*(z2_xz - z1_xz) + z1_xz*(y1_xz - y2_xz) ];
A_yz = [ (y2_yz - y1_yz), (x1_yz - x2_yz), 0;
(z2_yz - z1_yz), 0, (x1_yz - x2_yz);
0, (z2_yz - z1_yz), (y1_yz-y2_yz) ];
b_yz = [ x1_yz*(y2_yz - y1_yz) + y1_yz*(x1_yz - x2_yz);
x1_yz*(z2_yz - z1_yz) + z1_yz*(x1_yz - x2_yz);
y1_yz*(z2_yz - z1_yz) + z1_yz*(y1_yz - y2_yz) ];
A = [A_xz; A_yz];
b = [b_xz; b_yz];
c = A\b; % Solve for 3D coordinates
XYZ(i, 1) = XZ(i,2);
XYZ(i, 2) = c(1,1);
XYZ(i, 3) = c(2,1);
XYZ(i, 4) = c(3,1);
end
</nowiki>

==Finding Maxes and Mins, Polyfits==

The following Matlab function was used to calculate the minima of a set of data:

% Function locates the minima of a function, based on the 3 previous and
% 3 future points.
% Function takes one input, an [nx4] matrix where:
% ydisp(1,:) = image number from which data was taken.
% ydisp(2,:) = time at which data was taken.
% ydisp(3,:) = x or y position of object.
% ydisp(4,:) = z position (height of object).
% The function returns a matrix of size [nx3] containing the image number,
% time, and value of each local minimum.
function Mins = calc_minima(ydisp);
Mins = zeros(10, 3);
mat_size = size(ydisp);
num_points = mat_size(1,1);
local_min = ydisp(1, 4);
time = ydisp(1, 1);
location = 1;
index = 1;
min_found = 0;
for i = [4 : 1 : num_points - 3]
% execute if three previous points are greater in value
if ( ydisp(i, 4) < ydisp(i - 1, 4) && ydisp(i, 4) < ydisp(i - 2, 4) && ydisp(i, 4) < ydisp(i - 3, 4))
local_min = ydisp(i, 4);
time = ydisp(i, 1);
location = i;
% if next three points are also greater in value, must be a min
if ( ydisp(i, 4) < ydisp(i + 1, 4) && ydisp(i, 4) < ydisp(i + 2, 4) && ydisp(i, 4) < ydisp(i + 3, 4))
min_found = 1;
end
end
if (min_found)
Mins(index, 1) = location;
Mins(index, 2) = time;
Mins(index, 3) = local_min;
index = index + 1;
min_found = 0;
end
end

Once the minima were located, the following was used to compute a second order polyfit between two consecutive minima:

% plot_polyfit calls on the function calc_minima to locate the local mins
% of the data, and then calculates a parabolic fit in between each of two
% consecutive minima.
% It takes in raw_data in the form of an [nx4] matrix and returns
% a matrix containing the fitted data, the matrix of calculated minima, and
% the matrix of calculated maxima.
function [fittedData, Maxima, Mins] = plot_polyfit(raw_data);
num_poly = 2;
timestep = 1000;
fittedData = zeros(100,3);
Mins = calc_minima(raw_data); % Calculate minima
Maxima = zeros(10,2);
mat_size = size(Mins);
num_mins = mat_size(1,1);
index = 1;
max_index = 1;
% For each min, up until the second to last min, execute the following...
for i = [1 : 1 : num_mins - 1]
% Calculate coefficients, scaling, and translational factors for polyfit
[p,S,mu] = polyfit(raw_data(Mins(i, 1) : Mins(i+1, 1), 2), raw_data(Mins(i, 1) : Mins(i+1, 1), 4), num_poly);
start = index;
% Given the parameters for the polyfit, compute the values of the
% function for (time at first min) -> (time at second min)
for time = [raw_data(Mins(i, 1), 2) : timestep : raw_data(Mins(i + 1, 1), 2)]
fittedData(index, 1) = time;
x = [time - mu(1,1)] / mu(2,1);
y = 0;
v = 0;
% For each term in the polynomial
for poly = [1 : 1 : num_poly + 1]
y = y + [ p(1, num_poly - poly + 2) ]*x^(poly-1);
v = v + (poly-1)*[ p(1, num_poly - poly + 2) ]*x^(poly-2);
end
fittedData(index, 2) = y;
fittedData(index, 3) = v;
index = index + 1;
end
t = -p(1,2)/(2*p(1,1));
x = [t * mu(2,1)] + mu(1,1);
local_max = 0;
for poly = [1 : 1 : num_poly + 1]
local_max = local_max + [ p(1, num_poly - poly + 2) ]*t^(poly-1);
end
Maxima(max_index, 1) = x;
Maxima(max_index, 2) = local_max;
max_index = max_index + 1;
end

Robot Snake

2008-03-21T02:47:37Z

ClaraSmart: /* The Head Segment */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==

In this project, we developed and built a robot to mimic serpentine motion of a snake. The robot is made up of several body segments and a head. Each body segment contains a RC servo, which is controlled by a PIC microntroller located in the head of the snake robot. This wiki page contains discussions of the motion of a snake, mechanical design, electronic design and PIC code.

[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake]

[http://www.youtube.com/watch?v=r_GOOFLnI6w Video of the robot snake 2]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009
 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include serpentine movement, rectilinear movement, concertina movement and side-winding movement. The most common motion exhibited by most snakes is serpentine motion where each section follows a similar path (Ma, 205). In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficients of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction. (Saito et al, 66)

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of a snake robot are listed below:

*Move across uneven terrain, since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand, since it can distribute its weight across a wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

(Ma, 206)

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations (Saito etal, 72-73):

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

===Parts List===

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 1/2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis
*Ball caster: For the head

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

[http://www.youtube.com/watch?v=wBcJkNHEaAs Video of 3 body segments moving]

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chassis Built Showing a Standoff and Batteries]]
[[image:BuiltChasis2_MLS.jpg|thumb|right|Chassis with Batteries Removed]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries)
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
====Parts List (Digikey Part Number)====

*PIC: PIC18F4520
*Oscillator: 40MHz Oscillator (X225-ND)
*RC Servo (see mechanical design) preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G): 1 per segment
*10 pos IDC cable header (A26267-ND): 1 per segment
*3 pos AAA battery holder (BH3AAA-W-ND): 1 per segment
*2 pos AAA battery holder (BH2AAA-W-ND): 1 per segment
*475 Ohm resistors (transmission line termination)
*Various switches to turn power electronics and the motors on/off
*Standard Protoboard, to mount connector from ribbon cable, and switches for each motor
*Xbee radio pair and PC

====Electronics in Each Body Segment====
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]][[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]][[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line as shown in the ribbon cable schematic. Each segment of the snake contains a small circuit board (ServoBoard Schematic) which has a connector for the ribbon cable, a switch to control the power, and a power indicator LED. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

====Electronics in The Head Segment====
[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]

The PIC18F4520 Prototyping Board designed by Professor Peshkin was used. Schematics of the board can be found here: [[Main_Page#PIC_18F4520_prototyping_board|18F4520_prototyping_board]]. The only change applied to the board was to replace the 20MHz clock with a 40MHz clock. This allowed the microcontroller to perform calculations faster, improving the resolution of the servo signal. The ribbon cable was connected to the ground and port D pins on the PIC.

An [[XBee_radio_communication_between_PICs|XBee radio]] was used to communicate between the microcontroller and the PC. The wiring diagram shows a schematic for the Xbee connection with the PIC. The [[XBee_radio_communication_between_PICs#XBee_Interface_Module|XBee Interface Board]] was used to provide a robust mechanical mount for the radio, as well as supply the 3.3V needed by the XBee. On the PC side, another XBee interface board was plugged into the FTDI USB-Serial converter. Other than this, no special electronics were needed for the XBee radio. The radio simply acted as a serial cable replacement The snake was controlled by sending commands with a terminal program.

 

== PIC Code ==
There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and receives data from a computer via serial communication.

The main purpose of SnakeServos.c is to calculate the motion profile of the servos, and send a corresponding signal to each of the servos every 20 ms. The code for this is found in the <tt>ISR_20MS</tt> function in the code which is run every 20ms.

A secondary function is to update the parameters that affect the motion of the snake. The code for this can be found in the <tt>ISR_USART_RX</tt> function, which is run every time a byte is received on the USART's receive buffer.

====Servo Control Details====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS (defined as 15536), which will cause Timer1 to overflow 20ms later and re-trigger the interrupt. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS (defined as 15536 + 6250), Timer1 is polled, and the value is compared sequentially to the values in the RCservo array plus 15536 (because Timer1 started counting at 15536, not 0). If the value of Timer1 is greater than a value in (RCservo[x]+15536), the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although polling the timer to control the length of a pulse has a lower resolution than using an interrupt (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution for the pulse was about 8us, which was good enough for this purpose.

====Serial Communication Details====
The PIC communicates serially with a XBee radio to a PC with a XBee radio. As shown in the code, the serial communication allows the user to change the speed, the amplitude and period of the sine wave, and the direction (forward, reverse, left and right) of the robotic snake. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

//use volatile keyword to avoid problems with optimizer
volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS); //set timer to trigger an interrupt 20ms later
SET_ALL_SERVOS(0b11111111); //begin pulse for servo signal
time=get_timer1(); //poll timer
while(time < TMR1_2point25MS){ //end this loop after 2.25 ms
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0); //end the pulse when time is up
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1(); //poll timer
}
SET_ALL_SERVOS(0); //set all servos low in case some pins are still high

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/
RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed; //increment time, wrap around if necessary to prevent overflow
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
//load default values
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1); //enable Timer1 interrupt
enable_interrupts(INT_RDA); //enable USART receive interrupt
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here.

[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]
[http://www.youtube.com/watch?v=r_GOOFLnI6w Video of the robot snake 2]

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==

Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol 15, No 2 (2001): 205-6.

Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66, 72-73.

Robot Snake

2008-03-20T17:49:28Z

ClaraSmart: /* Results */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==

In this project, we developed and built a robot to mimic serpentine motion of a snake. The robot is made up of several body segments and a head. Each body segment contains a RC servo, which is controlled by a PIC microntroller located in the head of the snake robot. This wiki page contains discussions of the motion of a snake, mechanical design, electronic design and PIC code.

[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009
 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include serpentine movement, rectilinear movement, concertina movement and side-winding movement. The most common motion exhibited by most snakes is serpentine motion where each section follows a similar path (Ma, 205). In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficients of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction. (Saito et al, 66)

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of a snake robot are listed below:

*Move across uneven terrain, since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand, since it can distribute its weight across a wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

(Ma, 206)

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations (Saito etal, 72-73):

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

===Parts List===

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 1/2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis
*Ball caster: For the head

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries)
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
====Parts List (Digikey Part Number)====

*PIC: PIC18F4520
*Oscillator: 40MHz Oscillator (X225-ND)
*RC Servo (see mechanical design) preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G): 1 per segment
*10 pos IDC cable header (A26267-ND): 1 per segment
*3 pos AAA battery holder (BH3AAA-W-ND): 1 per segment
*2 pos AAA battery holder (BH2AAA-W-ND): 1 per segment
*475 Ohm resistors (transmission line termination)
*Various switches to turn power electronics and the motors on/off
*Standard Protoboard, to mount connector from ribbon cable, and switches for each motor
*Xbee radio pair and PC

====Electronics in Each Body Segment====
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]][[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]][[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line as shown in the ribbon cable schematic. At each motor, a small circuit board (ServoBoard Schematic) contains the connector for the ribbon cable, a switch to control the power and a power indicator LED. This circuit board has a common ground, connecting the signal ground with the battery ground and receives power from the batteries. The actual circuit board can be seen in the image (A Complete Circuit Board on the Snake). Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

====Electronics in The Head Segment====
[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]

The PIC18F4520 Prototyping Board designed by Professor Peshkin was used. Schematics of the board can be found here: [[Main_Page#PIC_18F4520_prototyping_board|18F4520_prototyping_board]].

The Xbee Radio

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. [http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==

Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol 15, No 2 (2001): 205-6.

Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66, 72-73.

Robot Snake

2008-03-20T16:58:59Z

ClaraSmart: /* Electronics */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
====Parts List (Digikey Part Number)====

*PIC: PIC18F4520
*Oscillator: 40MHz Oscillator (X225-ND)
*RC Servo (see mechanical design) preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*Various switches to turn power electronics and the motors on/off
*Standard Protoboard, to mount connector from ribbon cable, and switches for each motor
*Xbee radio pair and PC

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

====Electronics in Each Body Segment====
The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. At each motor, a small circuit board contains the connector for the ribbon cable, a switch to control the power and a power indicator LED. This circuit board has a common ground, connecting the signal ground with the battery ground and receives power from the batteries. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

====Electronics in The Head Segment====

The PIC /PIC board and power to the PIC Board

The Xbee Radio

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T16:00:57Z

ClaraSmart: /* Parts List (Digikey Part number) */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
====Parts List (Digikey Part Number)====

*PIC: PIC18F4520
*Oscillator: 40MHz Oscillator (X225-ND)
*RC Servo (see mechanical design) preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*Various switches to turn power electronics and the motors on/off
*Standard Protoboard, to mount connector from ribbon cable, and switches for each motor
*Xbee radio pair and PC

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

====Electronics in Each Body Segment====
The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. At each motor, a small circuit board contains the connector for the ribbon cable, a switch to control the power and a power indicator LED. This circuit board has a common ground, connecting the signal ground with the battery ground and receives power from the batteries. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

====Electronics in The Head Segment====

The PIC /PIC board and power to the PIC Board
The Xbee Radio

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T16:00:46Z

ClaraSmart: /* Parts List (Digikey Part number)= */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
====Parts List (Digikey Part number)====

*PIC: PIC18F4520
*Oscillator: 40MHz Oscillator (X225-ND)
*RC Servo (see mechanical design) preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*Various switches to turn power electronics and the motors on/off
*Standard Protoboard, to mount connector from ribbon cable, and switches for each motor
*Xbee radio pair and PC

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

====Electronics in Each Body Segment====
The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. At each motor, a small circuit board contains the connector for the ribbon cable, a switch to control the power and a power indicator LED. This circuit board has a common ground, connecting the signal ground with the battery ground and receives power from the batteries. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

====Electronics in The Head Segment====

The PIC /PIC board and power to the PIC Board
The Xbee Radio

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T16:00:23Z

ClaraSmart: /* Electronics */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
====Parts List (Digikey Part number)=====

*PIC: PIC18F4520
*Oscillator: 40MHz Oscillator (X225-ND)
*RC Servo (see mechanical design) preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*Various switches to turn power electronics and the motors on/off
*Standard Protoboard, to mount connector from ribbon cable, and switches for each motor
*Xbee radio pair and PC

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

====Electronics in Each Body Segment====
The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. At each motor, a small circuit board contains the connector for the ribbon cable, a switch to control the power and a power indicator LED. This circuit board has a common ground, connecting the signal ground with the battery ground and receives power from the batteries. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

====Electronics in The Head Segment====

The PIC /PIC board and power to the PIC Board
The Xbee Radio

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:50:46Z

ClaraSmart: /* SnakeServos.c */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

====SnakeServos.c====
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:50:26Z

ClaraSmart: /* Serial Communication */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

====Serial Communication====
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:49:59Z

ClaraSmart: /* Servo Control */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

====Servo Control====
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1's counter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:48:31Z

ClaraSmart: /* High Torque Servos */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

====High Torque Servos====
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:48:18Z

ClaraSmart: /* Power Supply */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

==== Power Supply ====
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:47:50Z

ClaraSmart: /* Obstacle Avoidance */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

==== Obstacle Avoidance ====
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:47:31Z

ClaraSmart: /* Position Sensors */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

==== Position Sensors ====
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:47:14Z

ClaraSmart: /* Position Sensors */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course or maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:46:03Z

ClaraSmart: /* Results */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels, as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

Wireless control from a laptop allowed easy demonstration of the snakes capabilities, and allowed others to easily control its movement.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course of maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:43:25Z

ClaraSmart: /* Protection and Visual Appeal */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]
As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course of maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:42:25Z

ClaraSmart: /* Parts List */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

====Parts List====

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 

==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course of maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:41:27Z

ClaraSmart: /* Standoffs */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

===Parts List===

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

====Standoffs ====

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 
==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

==== Protection and Visual Appeal ====
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course of maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:40:51Z

ClaraSmart: /* Standoffs */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

===Parts List===

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

== Standoffs ==

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 
==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 

=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

=== Protection and Visual Appeal ===
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course of maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-20T15:40:11Z

ClaraSmart: /* Mechanical Design */

[[image:Snake_Robot_1.jpg|right]]

== Overview ==
[http://www.youtube.com/watch?v=Sb8WqaLX1Vo Video of the robot snake.]

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

===Parts List===

*Motors: Futaba S3004 standard ball bearing RC servo motor, Tower Hobbies LXZV41 $12.99
*Wheels: McMasterCarr Acetal Pulley for Fibrous Rope for 1/4" Rope Diameter, 3/4" OD McMasterCarr 8901T11 $1.66
*O-Rings (Tires): McMasterCarr Silicone O-Ring AS568A Dash Number 207, Packs of 50 McMasterCarr 9396K209 $7.60/50
*PVC Pipe: McMasterCarr Sewer & Drain Thin-Wall PVC Pipe Non-Perforated, 3" X 4-1/2' L, Light Green McMasterCarr 2426K24 $7.06
*1/8th inch plastic for chassis: (Shop Stock) or McMasterCarr Polycarbonate Sheet 1/8" Thick, 12" X 12", Clear, McMasterCarr, 8574K26 $6.32
*Dowel Pins: 1" long, 1/4" diameter
*Sheet Metal: For the connecting segments
*Fasteners: Screws for the servos and chassis, washers for the standoffs
*Standoffs: Used 1" and 2" to achieve a level snake
*Velcro: To attach battery packs and housing to the chasis

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

=== Standoffs ===

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 
==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 
=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

=== Protection and Visual Appeal ===
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==
Parts (Digikey Part number)

*PIC18F4520
*40MHz Oscillator (X225-ND)
*RC Servo,preferably high-torque
*10 wire IDC ribbon cable
*10 pos IDC cable socket (ASC10G)
*10 pos IDC cable header (A26267-ND)
*3 pos AAA battery holder (BH3AAA-W-ND)
*2 pos AAA battery holder (BH2AAA-W-ND)
*475 Ohm resistors (transmission line termination)
*various switches

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal. The signal generated by the microcontroller is carried by the IDC ribbon cable, and each servo board taps into a single signal line and the reference ground line. Because of the length of the ribbon cable, each signal line must be terminated with a 475 ohm resistor to prevent reflected "ghost" signals from interfering with the original signal.

Each servo board also has its own power supply of 5 AAA cells, which gives each servo 7.5V. Although the servos are only rated for 6V, 7.5V was used because more torque was needed. The current drain (up to 500mA) caused the voltage across the cells to drop due to the high internal resistance of the alkaline cells. NiMH rechargeable cells are more capable of handling high current draw applications, but are also much more expensive and can take several hours to charge.

The robot snake can run for about 1 hour on the alkaline cells, after which the servos no longer have enough torque to generate the serpentine motion.

 

== PIC Code ==

There are two PIC files used in this robotic snake, SnakeServos.c and main.h, which are shown below. main.h sets up the default parameters used in SnakeServos.c. The microcontroller controls the RC servos and communicates via serial communication to a computer. These two functions are discussed below.

===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1 is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated. As shown in the code, the serial communication allows the user to change the speed, the amplitude and phase of the sinewave, and the direction (forward, reverse, left and right) of the robotic snake.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Overall, the robotic snake was successful.

Initially, the mechanical design included a single wheel mounted in the center of the pvc pipe. However, the motion of the snake was very difficult to control because the robotic snake became unstable very easily. As a result, the chassis was built to include two wheels as discussed in the mechanical design section, in order to provide stability which made the robot easier to control.

The final robotic snake can be seen in action here. (insert link for youtube video)

== Next Steps ==

The robotic snake was developed within five weeks, and proved to be a very successful demo. There are many options that could be researched and developed to add to this robot and discussed below.

=== Position Sensors ===
Sensors could be added to the robot to allow it to know its position. This could be accomplished with a combination of encoders on a segment. Most likely, the middle segment should be used since it would be the approximate center of gravity. Knowledge of the position of the center of gravity would potentially the robotic snake to be sent to different locations or navigate (using dead reckoning) through a pre-determined obstacle course of maze. The information from encoders could be sent to a computer to observe different snakelike motions with different parameters.

=== Obstacle Avoidance ===
With optical sensors on the head of the snake, the robot would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

=== Power Supply ===
Currently, 5 AAA batteries are required for each servo, meaning that this robot requires many batteries. As a result, a different power supply could be investigated.

===High Torque Servos===
The servos in the snake have a large load but do not need to move very quickly, so high torque servos should be used instead of standard servos. This would also prolong the battery life because the servos would be operating in a more efficient range.

== References ==
</references>

Robot Snake

2008-03-19T15:17:36Z

ClaraSmart: /* Team Members */

[[image:Snake_Robot.jpg|center]]

<center> Snake Robot </center>

== Overview ==

==Team Members==

[[image:Team23_Members.jpg|thumb|400pix|right|Hwang-Long-Smart]]

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

=== Standoffs ===

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 
==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 
=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

=== Protection and Visual Appeal ===
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal.

Needs description of servo - include discussion of how we aligned them (through code)

Needs discussion on batteries. Rechargeable vs Alkaline, 4 vs 5 etc.

Needs circuit diagram for servo boards and/or picture

Needs description of ribbon cable? (ie be careful with it)

Needs reason for terminator

 

== PIC Code ==
===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Initial Design Discussion? May not be necessary

Discuss how it went well

Link to you tube video (s)

== Next Steps ==

This snake was developed within five weeks, and proved to be a very successful demo.

'''Further capabilities of the snake could include:'''

Sensors to determine position: This could be accomplished with a combination of encoders on the middle segment (which would detect forward motion, and the head (which would detect current direction). This type of awareness would allow the snake to be sent to different locations or navigate (using dead reckoning) though a pre-determined obstacle course of maze. These encoders would also allow for the different snakelike motions to be observed, for instance, how does changing only the amplitude affect the speed?

Obstacle avoidance: With optical sensors on the head of the snake, it would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

Power supply: Currently 5 AAA batteries supply each servo, thus the snake uses way too many batteries. A different power supply could be investigated.

== References ==
</references>

Robot Snake

2008-03-19T15:12:40Z

ClaraSmart: /* Next Steps */

[[image:Snake_Robot.jpg|center]]

<center> Snake Robot </center>

== Overview ==

==Team Members==

*Michael Hwang - Electrical Engineer - Class 2008
*Andrew Long - Mechanical Engineer - Class 2009
*Clara Smart - Electrical Engineer - Class 2009

[[image:Team23_Members.jpg|thumb|400pix|center|Hwang-Long-Smart]]

 

== Snake Motion ==
[[image:Snake_Motion.jpg|thumb|right|Source: [http://science.howstuffworks.com/snake3.htm How Stuff Works]]]
Snakes are able to adapt their movement to various environments. For instance, snakes can move across extreme environments such as sand, mud and water. Research has discovered there are four types of snake motion, as shown in the image. These motions include; serpentine movement, rectilinear movement, concertina movement and side-winding movement.<ref>Ma, Shugen. "Analysis of creeping locomotion of a snake-like robot." ''Advanced Robotics'' Vol.15, No.2 (2001): 205</ref> The most common motion exhibited by most snakes is serpentine motion where section follows a similar path. In order for snakes to successfully locomote using serpentine motion, the belly of the snake must have anisotropic coefficient of friction for the normal and tangential directions. Specifically, the normal friction must be greater than the tangential friction. As a result, when the snake exhibits a force on the ground, it will move in the tangential direction without slipping in the normal direction.<ref>Saito, Fukaya, Iwasaki. "Serpentine Locomotion with Robotic Snakes". ''IEEE Control Systems Magazine'' (Feb 2002): 66.<ref/>

 

== Advantages / Disadvantages of Robotic Snake Motion ==

===Advantages===

Many robots are limited by the use of motorized wheels. However, there are many advantages for building a robot that mimics the motion of a snake. Several advantages for movement of snake robot are listed below:

*Move across uneven terrain since it is not dependent on wheels
*Possibly swim if water-proofed
*Move across soft ground such as sand since it can distribute its weight across wider area

Also, from a systems standpoint, the snake robot can be very modular with many redundant segments. As a result, it is very easy to replace broken segments as well as shorten or lengthen the robot.

===Disadvantages===

Although there are many advantages for building a snake like robot, there are several disadvantages which are listed below:

*Low power and movement efficiency
*High cost of actuators (servos or motors)
*Difficult to control high number of degrees of freedom

Cite = Page 206

== Robot Snake Motion ==
[[image:Serpentine_curves.jpg|thumb|300pix|right|Serpentine Curves]]

Real snake motion does not follow specified equations. However, research has proven that the serpentine motion of a snake can be modeled with the following equations:

<math>x(s)= \int_{0}^{s} \cos (\zeta_\sigma) d\sigma</math>

<math>y(s)= \int_{0}^{s} \sin (\zeta_\sigma) d\sigma </math>

<math>\zeta_\sigma= a \cos (b\sigma) +c\sigma </math>

where the parameters ''a'', ''b'', and ''c'' determine the shape of the serpentine motion. The graph shows how the parameters influence the serpentine curve. Basically, ''a'' changes the appearance of the curve, ''b'' changes the number of phases, and ''c'' changes the direction.

The serpentine curve can be modeled with a snake like robot by changing the relative angles between the snake robot segments using the following formula with the number of segments (n):

<math>\phi_i = \alpha sin(\omega t +(i-1)\beta ) + \gamma, \left ( i=1, ..., n-1 \right )</math>

where α , β , and γ are parameters used to characterize the serpentine curve and are dependent on ''a'', ''b'', and ''c'' as shown below:

<math>\alpha = a \left | \sin \left ( \frac{\beta}{2} \right ) \right | </math>

<math>\beta = \frac{b}{n} </math>

<math>\gamma = -\frac{c}{n} </math>

The equations above for φi,α,β, and γ were used in this snake like robot as shown in the [[Robot Snake#PIC Code|code section]].

 

== Mechanical Design ==
[[image:FullSnake.jpg|thumb|right|The Snake]]
The robotic snake consists of a head segment and several body segments. The head segment houses the onboard microcontroller and xBee radio. The body segments house the servo motors and the batteries required to power each motor. As the snake is designed to be modular, there is no limit to the number of body segments. More segments will allow it to move more smoothly, while fewer segments will be easier to control. For this design, seven body segments were used due to material limitations.

Mechanically, the snake is designed to move in a serpentine motion, imitating the motion of a real snake. As discussed above, real snakes move with anisotropic coefficients of friction. It is difficult to locate materials with this property, but passive wheels satisfy the friction requirements. The friction will be lower in the direction of rolling, thus providing the required difference in friction. The only problem with this approach is that the wheel may slide in the normal direction if the weight applied to the wheel is not sufficient.

=== The Body Segments ===
[[image:Chasis.jpg|thumb|right|A Single Chasis Without a Servo]]

Each of the body segments are identical and includes a chassis, a servo, a connector, standoffs and two passive wheels as can be seen in the picture.

==== Chassis ====

The base of the chassis is made from a thin (approx. 1/8th inch) piece of polycarbonate. The chassis must be wide enough to hold a servo motor with a AAA battery pack on each side and long enough for the servo and a standoff (the connection for the previous segment). The polycarbonate was cut into a rectangle to meet the specifications for our servo motor. Five holes were then drilled in the rectangle, four to mount the servo and one for the standoff. The holes are drilled to allow the servo to be located in the center of the chassis.

==== Connector ====

A connector was machined to attach to the servo horn of one body segment and to attach to the next segment's standoff. The length of this connector is about 3 inches and is just long enough to prevent collision between segments. A shorter beam allows for greater torque. This connection needs to be as tight as possible and the beam must be mounted perpendicular to the chassis.

[[image:ChasisUnderside.jpg|thumb|right|The Underside of a Chassis]]

=== Standoffs ===

Standoffs were used to attach the servo to the chassis and to attach the connector to the chassis. Two standoffs (1 in and 1/2 in) and several washers were used to make the connector parallel to the ground.

==== Passive Wheels ====
[[image:Wheel.jpg|thumb|left|A Passive Wheel on the Dowel Pin]]
Passive wheels were mounted to the bottom of the chassis. Each wheel was made of a 3/4 inch pulley and an o-ring. The o-ring was used to increase friction with the ground. The wheels have been set on polished metal dowel pins which allow the wheels to rotate more freely than when placed on wooden dowels. The dowel pin axles were mounted (hot glue works but is not very strong) in the center of the segment. The center of the segment is not the center of the polycarbonate rectangle. Instead, the entire segment length is the distance from the standoff on one chassis to the center of the servo horn on the other. In this project, the length of the connector was made to be about half the length of the segment. Therefore, the wheels were placed at the same location as the stand off as can be seen in the image. The wheels are held in place with zip ties.
 
==== Fully Assembled Body Segment ====
[[image:BuiltChasis.jpg|thumb|right|A Chasis Built Showing a Standoff and Batteries]]
A fully assembled chassis has a mounted servo and is connected to a segment on either side. AAA batteries packs were attached to the sides of the motor with velcro to allow easy removal. The small electronic circuit board for each segment was mounted on the front of the motor to allow easy access to the switch. (See Electronic Design for more information on the circuit board and batteries.
 
=== The Head Segment ===
[[image:BallCaster.jpg|thumb|left|The Ball Caster Under the Front Segment]]

The head segment is similar to the body segments except that it contains a PCB board with a PIC instead of a servo motor. The head segment is the same width but slightly longer than the body segment. A ball caster was added to the front of the segment to help support the extra length and help the wheels stay on the ground.
 

=== Protection and Visual Appeal ===
[[image:Housing.jpg|thumb|right|One Segment of the Housing]]

As a final step, housing for each segment was created from 3" PVC pipe. The pipe was cut into segments the same length as the chassis. The bottom of the pipe was cut off, allowing it to sit flat on the chassis. The housing provides a protective covering for the servo, batteries and electronics. The pipe was attached with velcro straps which mounted under the chassis. This housing can be easily removed to debug and to change batteries.
 

=== Mechanical Debugging ===

Wheels come off the ground: Add washers to the standoffs to force the chassis to be parallel to the ground.

Wheels slide, but do not roll: Increase frictionby either adding weight to the segment or changing the "tires" (the o-ring).

The segments slip when the servo rotates: Tighten the screws for the connector standoffs, both above the beam and below the chassis.
 

== Electronics ==

[[image:PICBoard_schematic_HLS.jpg|thumb|right|The Mainboard Schematic]]
[[image:RibbonCable_schematic_HLS.jpg|thumb|right|Ribbon Cable Schematic]]
[[image:ServoBoard_schematic_HLS.jpg|thumb|right|ServoBoard Schematic]]

[[image:PICBoard_HLS.jpg|thumb|right|The Electronics in the Head]]
[[image:ServoBoard_Hooked_up_HLS.jpg|thumb|right|A Complete Circuit Board on the Snake]]

The each segment of the snake contains a Futaba Standard RC Servo. Each servo has 3 wires: power, ground, and signal.

Needs description of servo - include discussion of how we aligned them (through code)

Needs discussion on batteries. Rechargeable vs Alkaline, 4 vs 5 etc.

Needs circuit diagram for servo boards and/or picture

Needs description of ribbon cable? (ie be careful with it)

Needs reason for terminator

 

== PIC Code ==
===Servo Control===
The main function of the PIC microcontroller was to control multiple servos (seven in our case). Timer1is set to overflow every 20 milliseconds and trigger an interrupt. When the interrupt is triggered, Timer1'scounter is set to the value held by TMR1_20MS, which will cause the interrupt to trigger again 20 ms later. At the beginning of the interrupt, all the pins connected to the servos are set high. While Timer1 is less than the value held by TMR1_2point25MS, Timer1 is polled, and the value is compared sequentially to the values in the RCservo array. If the value of Timer1 is greater than a value in RCservo, the the corresponding pin is set low. After all the values have been compared, Timer1 is polled again and the process repeats until 2.25 ms have elapsed (when Timer1 > TMR1_2point25MS). After all the servos signals have been sent, the values in the RCServo array are updated to prepare it for the next 20ms interrupt.

Although this method of timing the pulse trains has a lower resolution than using interrupts (see [http://peshkin.mech.northwestern.edu/pic/code/RCservoSoft/RCservoSoft.c RCservoSoft.c]), it allows one to add and remove servos more easily and not have to decrease the frequency of the servo signal pulse train. With a 40MHz clock and seven servos, the resolution was about 8us, which was good enough for this purpose.

===Serial Communication===
The PIC communicates serially with a XBee radio. When a byte is received in the UART receive buffer, a high-priority interrupt is triggered. The received byte is put into a switch-case statement, and the corresponding parameters are updated.

===SnakeServos.c===
<pre>
/*
Andy Long, Clara Smart, and Michael Hwang's snake robot code.
*/

#include <18f4520.h>
#device high_ints=TRUE // this allows raised priority interrupts, which we need
#fuses HS,NOLVP,NOWDT,NOPROTECT
#use delay(clock=40000000)
#use rs232(baud=9600, UART1)

#include <main.h>
#include <math.h>

/*
Put your desired high duration here;
3200 is center
1000 is 90 deg right
5400 is 90 deg left
*/
int16 RCservo[7];

volatile float a = A_DEFAULT;
volatile float b = B_DEFAULT;
volatile float c = C_DEFAULT;

volatile float alpha;
volatile float gamma;
volatile float beta;
volatile float speed = 0;
volatile float prev_speed = SPEED_DEFAULT;
float t = 0;

#INT_TIMER1 // designates that this is the routine to call when timer1 overflows
//generates servo signals
void ISR_20MS(){
volatile unsigned int16 time;
set_timer1(TMR1_20MS);
SET_ALL_SERVOS(0b11111111);
time=get_timer1();
while(time < TMR1_2point25MS){
if (time > (RCservo[0] + TMR1_20MS)){
output_low(SERVO_0);
}
if (time > (RCservo[1] + TMR1_20MS)){
output_low(SERVO_1);
}
if (time > (RCservo[2] + TMR1_20MS)){
output_low(SERVO_2);
}
if (time > (RCservo[3] + TMR1_20MS)){
output_low(SERVO_3);
}
if (time > (RCservo[4] + TMR1_20MS)){
output_low(SERVO_4);
}
if (time > (RCservo[5] + TMR1_20MS)){
output_low(SERVO_5);
}
if (time > (RCservo[6] + TMR1_20MS)){
output_low(SERVO_6);
}
time=get_timer1();
}
SET_ALL_SERVOS(0);

//3200 is center //1000 is 90 deg right // 5400 is 90 deg left
/*
add value of sine wave with phase offset ((alpha*sin(t + X*beta),
3200 for servo center position,
an adjustment value to compensate for offsets when mounting servo horn (SERVO_X_ADJ),
and bias (gamma) for turning.
*/

RCservo[0]=(int16)(alpha*sin(t) + 3200 + SERVO_3_ADJ + gamma);
RCservo[1]=(int16)(alpha*sin(t + 1*beta) + 3200 + SERVO_4_ADJ + gamma);
RCservo[2]=(int16)(alpha*sin(t + 2*beta) + 3200 + gamma + SERVO_5_ADJ);
RCservo[3]=(int16)(alpha*sin(t + 3*beta) + 3200 + gamma + SERVO_6_ADJ);
RCservo[4]=(int16)(alpha*sin(t + 4*beta) + 3200 + gamma + SERVO_7_ADJ);
RCservo[5]=(int16)(alpha*sin(t + 5*beta) + 3200 + gamma + SERVO_8_ADJ);
RCservo[6]=(int16)(alpha*sin(t + 6*beta) + 3200 + gamma + SERVO_9_ADJ);

t+= speed;
if (t > 2*pi){
t = 0;
}
else if (t < 0){
t = 2*pi;
}
}

#INT_RDA HIGH //High-Priority Interrupt triggered by USART Rx
//parameter update
void ISR_USART_RX(){
char input;
if (kbhit()){
input = getc();
switch(input){
case 'w': //accelerate
speed += 0.002;
break;
case 's': //decelerate
speed -= 0.002;
break;
case 'x': //pause motion
prev_speed = speed;
speed = 0;
break;
case 'z': //resume motion
speed = prev_speed;
break;
case 'c': //reverse speed
speed = -speed;
break;
case 'a': //increase left turn rate
c -= 1000;
gamma=-c/num_segments;
break;
case 'd': //increase right turn rate
c += 1000;
gamma=-c/num_segments;
break;
case 'f': //set turn rate to 0
c = C_DEFAULT;
gamma = 0;
case 't': //increase amplitude
a += 10;
alpha=a*abs(sin(beta));
break;
case 'g': //decrease amplitude
a -= 10;
alpha=a*abs(sin(beta));
break;
case 'y': //increase phases in body
b += 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case 'h': //decrease phases in body
b -= 0.1;
beta=b/num_segments;
alpha=a*abs(sin(beta));
break;
case '1': //preset 1
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '2': //preset 2
a = 1400;
b = 2*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
case '3': //preset 3
a = 1000;
b = 5*pi;
c = C_DEFAULT;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=SPEED_DEFAULT;
break;
default:
}
}
return;
}

void main() {
a = A_DEFAULT;
b = B_default;
c = C_default;
gamma=-c/num_segments;
beta=b/num_segments;
alpha=a*abs(sin(beta));
speed=0;

setup_timer_1(T1_INTERNAL | T1_DIV_BY_4 );
set_timer1(0);

enable_interrupts(INT_TIMER1);
enable_interrupts(INT_RDA);
enable_interrupts(GLOBAL);

while (TRUE) {

}
}
</pre>

===main.h===
<pre>
#ifndef __MAIN_H__
#define __MAIN_H__

#define SET_ALL_SERVOS(x) output_d(x)

/*
This chart matches the pin on the PIC to the wire on the ribbon cable
PIN WIRE IN USE
--- ---- -------
RD0 2
RD1 3 *
RD2 4 *
RD3 5 *
RD4 6 *
RD5 7 *
RD6 8 *
RD7 9 *

*/
#define SERVO_3_ADJ 0
#define SERVO_4_ADJ 300
#define SERVO_5_ADJ (-150)
#define SERVO_6_ADJ 75
#define SERVO_7_ADJ (-200)
#define SERVO_8_ADJ 100
#define SERVO_9_ADJ (-150)

#define SERVO_0 PIN_D1
#define SERVO_1 PIN_D2
#define SERVO_2 PIN_D3
#define SERVO_3 PIN_D4
#define SERVO_4 PIN_D5
#define SERVO_5 PIN_D6
#define SERVO_6 PIN_D7

#define A_DEFAULT 1300
#define B_DEFAULT 3*pi
#define C_DEFAULT 0

#define SPEED_DEFAULT 0.05
#define OMEGA_DEFAULT 1
#define num_segments 8

#define TMR1_20MS 15536
#define TMR1_2point25MS 15536 + 6250
#endif
</pre>

== Results ==

Initial Design Discussion? May not be necessary

Discuss how it went well

Link to you tube video (s)

== Next Steps ==

This snake was developed within five weeks, and proved to be a very successful demo.

'''Further capabilities of the snake could include:'''

Sensors to determine position: This could be accomplished with a combination of encoders on the middle segment (which would detect forward motion, and the head (which would detect current direction). This type of awareness would allow the snake to be sent to different locations or navigate (using dead reckoning) though a pre-determined obstacle course of maze. These encoders would also allow for the different snakelike motions to be observed, for instance, how does changing only the amplitude affect the speed?

Obstacle avoidance: With optical sensors on the head of the snake, it would be able to sense an obstacle and either overide the wireless command and avoid it, or stop completely, and wait for further commands.

Power supply: Currently 5 AAA batteries supply each servo, thus the snake uses way too many batteries. A different power supply could be investigated.

== References ==
</references>

File:HLSSnakeMain.jpg

2008-03-19T14:54:16Z

ClaraSmart:

ME 333 final projects

2008-03-19T14:51:37Z

ClaraSmart: /* Robot Snake */

'''[[ME 333 end of course schedule]]'''

__TOC__

== ME 333 Final Projects 2008 ==

=== [[IR Tracker]] ===

[[Image:IR_Tracker_Main.jpg|right|thumb|200px]]

The IR Tracker (aka "Spot") is a device that follows a moving infrared light. It continuously detects the position of an infrared emitter in two axises, and then tracks the emitter with a laser.

 

=== [[Robot Snake]] ===
[[Image:HLSSnakeMain.jpg|right|thumb|200px]]

A wirelessly controlled robotic snake which uses a traveling sine wave and servo motors to mimic serpentine motion. The snake is capable of going forward, left, right and in reverse.

 

=== [[Programmable Stiffness Joint]] ===

The Programmable Stiffness Joint varies rotational stiffness as desired by the user. It is the first step in modeling the mechanical impedance of the human ankle joint (both stiffness and damping) for the purpose of determining the respective breakdown of the two properties over the gait cycle.

 

=== [[Magnetic based sample purification]] ===

 

=== [[Continuously Variable Transmission]] ===

[[image:CVT_setup1.jpg|thumb|200px]]

A continuously variable tramsission is intended to provide a transition from low to high gear ratios while keeping the engine input running at the max efficient speed. It is achieved by a system of variable radius pulleys and a v-belt.

 

=== [[Granular Flow Rotating Sphere]] ===

This device will be used to study the granular flow of particles within a rotating sphere. The sphere is filled with grains of varying size and then rotated about two different axes according to a series of position and angular velocity inputs.

 

=== [[WiiMouse]] ===

[[Image:HPIM1027.jpg|right|thumb|200px]]

The WiiMouse is a handheld remote that can be used to move a cursor on a windows-based PC, via accelerometer input captured through device movement.

 

=== [[Intelligent Oscillation Controller]] ===

This device "learns" a forcing function that is applied to a spring and mass system to match an arbitrary, periodic acceleration profile.

=== [[Baseball]] ===

[[Image:Baseball_Playfield.jpg|right|thumb|200px]]

An interactive baseball game inspired by pinball, featuring pitching, batting, light up bases and a scoreboard to keep track of the game.

Robot Snake

2008-03-16T15:17:10Z

ClaraSmart: /* Results */

Robot Snake

2008-03-16T05:27:44Z

ClaraSmart: /* Electronics */

Robot Snake

2008-03-16T05:25:39Z

ClaraSmart: /* Mechanical Design */

Robot Snake

2008-03-16T05:25:00Z

ClaraSmart: /* Mechanical Design */

Robot Snake

2008-03-16T05:24:12Z

ClaraSmart: /* Electronics */

Robot Snake

2008-03-16T05:23:42Z

ClaraSmart: /* Mechanical Design */

Robot Snake

2008-03-16T05:22:17Z

ClaraSmart: /* Mechanical Design */

Robot Snake

2008-03-16T05:18:28Z

ClaraSmart: /* Mechanical Design */