Robotics Final Project - RAJAT MAHAJAN

Code

The following code files should be put at WiiLAB\WiiLAB_Matlab\DemoPrograms :

% RAJAT MAHAJAN

% UID - 653805981

% Final Project

% ROBOTICS - Algorithms and Control (ECE 452)

%% Robo_final.m

% the main file - reads wiimote data and accordingly calls move function which controls the servo motors

clear all

% % % Open new NXT connection according to the previous generated configuration file.

handle = COM_OpenNXT();

COM_SetDefaultNXT(handle);

% Add the path containing simple graphics functions

addpath C:\'Program Files'\WiiLAB\WiiLAB_Matlab\EG111-H

% Add the path containing the Wiimote functions

addpath C:\'Program Files'\WiiLAB\WiiLAB_Matlab\WiimoteFunctions

% Add the paths for the Wiimote and Graphics Functions

addpath C:\'Program Files'\Wiimote\WiiLAB_Matlab\WiimoteFunctions

addpath C:\'Program Files'\Wiimote\WiiLAB_Matlab\EG111-H

% Array for holding X,Y, Z acceleration values

gesture = [];

% Counter used to store X,Y,Z acceleration values in a text file for input

% to Neural Network

k=0;

% Connect to the Wiimote

initializeWiimote();

% Check to make sure the Wiimote is connected

if( isWiimoteConnected() > 0 )

% Main program loop

% Used for polling the Wiimote data

% This loop will run until 'HOME' on the Wiimote is Pressed

while(~isButtonPressed('HOME'))

[x_accel y_accel z_accel] = getWiimoteAccel();

% Check if button B was pressed

if(isButtonPressed('B'))

if(x_accel > 0 && z_accel > 0 && (y_accel > -1 && y_accel < 1) )

display('right')

move(4); % Call move function - Controls the servo motors

elseif(x_accel < 0 && z_accel > 0 && (y_accel > -1 && y_accel < 1) )

display('left')

move(3);

elseif(y_accel > 0 && (z_accel < 0) && (x_accel >-1 && x_accel <1) )

display('forward')

move(1);

elseif(y_accel < 0 && (z_accel > 0)&& (x_accel >-1 && x_accel < 1) )

display('back')

tic

move(2);

toc

end

% Check if Button A was pressed - For Neural Network Based Getsure Recognition

if(isButtonPressed('A'))

% Arrays for holding X,Y,Z acceleration values

X = [];

Y =[];

Z = [];

T = [];

[X Y Z T] = collectAccelData(3,1);

% figure

% subplot(3,1,1);

% plot(T,X);

% subplot(3,1,2);

% plot(T,Y);

% subplot(3,1,3);

% plot(T,Z);

% k= k+1

tic

gesture(1:4) = X ;

gesture(5:8) = Y ;

gesture(9:12) = Z ;

% Code for stiring X,Y,Z acceleration data in text files

% gfile = strcat('gest',num2str(k),'.txt');

% gest = fopen(gfile,'wt');

% fprintf(gest, '%f ', gesture);

% fclose(gest);

% Call the Neural Network function

gesture_count = nnCP(gesture);

move(gesture_count);

toc

end

% If Up button on Wiimote was pressed Stop all Motors

if(isButtonPressed('UP'))

StopMotor('all', 'off');

end

% Pause - This is a required step. If it is not included you will

% experience unexpected behavior.

pause(.001);

end % while

else

% The Wiimote was not successfully connected

message = [ 'There was a problem with connecting to the Wiimote. ' ...

'Please make sure the Wiimote is properly connected ' ...

'to the computer and rerun the program.' ];

% This will write the message to the command line

disp(message);

% This will create a message box with the message

uiwait(msgbox(message, 'Connection Problem', 'error'));

end

% Disconnect the Wiimote

disconnectWiimote();

% StopMotor all off

% Close Bluetooth connection.

COM_CloseNXT(handle);

% RAJAT MAHAJAN

% UID - 653805981

% Final Project

% ROBOTICS - Algorithms and Control (ECE 452)

% move .m

% File to control the servo motors

function [] = move(m)

SetMotor 0

SyncToMotor 1

SetAngleLimit 0 % 0 means no angle limit = run forever

ResetMotorAngle(0);

ResetMotorAngle(1);

switch m

case 1

SetPower -100

SetTurnRatio 0

case 2

SpeedRegulation on

SetPower -100

SetTurnRatio 50

case 3

SetMotor 1

SyncToMotor 0

SpeedRegulation on

SetPower -100

SetTurnRatio 50

case 4

SetPower 100

SetTurnRatio 0

end

SendMotorSettings

pause(10);

StopMotor('all', 'off');

end

% RAJAT MAHAJAN

% UID - 653805981

% Final Project

% ROBOTICS - Algorithms and Control (ECE 452)

%% This is the Counter Propogation Neural Network Function, there are other functions which it

% calls, for details see below

% nnCP.m

function [number] = nnCP(gesdata)

% function cp= nnCP

% Get the training data

[trainingData, classNumber] = getCPTrainingData;

%% Create a default CP network

load cp_kohonen_weights;

load cp_grossberg_weights;

outputLen = length(trainingData(1).output);

cp = createDefaultCP([12, classNumber, outputLen]);

% % Training the CP network

[cp] = trainingCP(cp, trainingData);

% %test the original training data set;

% str = [];

% tdSize = size(trainingData);

% for n = 1: tdSize(2);

% [cp, output] = propagatingCP(cp, trainingData(n).input(:));

% [outputName, outputVector, outputError, outputClassID] = cpClassifier(cp,trainingData);

% if strcmp(outputName, trainingData(n).name)

% astr = [num2str(n), '==> Succeed!! The Error Is:', num2str(outputError)];

% else

% astr = [num2str(n), '==> Failed!!'];

% end

% str = strvcat(str, astr);

% end

% output = str;

% display(output);

% Test on the testing data

testingData(1).input = gesdata;

tedSize = size(testingData);

for n = 1: tedSize(2)

[cp, outputClassID] = propagatingCP(cp, testingData(n).input(:));

end

display('GESTURE');

display(num2str(outputClassID));

number = outputClassID;

% cpClassifier.m

function [outputName, outputVector, outputError, outputClassID] = cpClassifier(cp, trainingData)

outputName = [];

outputVector = [];

if nargin < 2

display('cpClassifier.m needs at least two parameter');

return;

end

dError = [];

dataSize = size(trainingData);

output = cp.grossberg.output;

for dataInd =1 : dataSize(2)

aSet = trainingData(dataInd).output(:);

vDiff = abs(aSet - output);

vDiff = vDiff.*vDiff;

newError = sum(vDiff);

dError = [dError, newError];

end

if ~isempty(dError)

[eMin, eInd] = min(dError);

outputName = trainingData(eInd).name;

outputVector = trainingData(eInd).output;

outputError = eMin;

outputClassID

% createDefaultCP.m

function cp = createDefaultCP(neuronNumberVector)

cp = [];

if nargin < 1

display('createDefaultCP.m needs one parameter');

return;

end

nSize = length(neuronNumberVector);

if nSize ~= 3

display('error parameter when calling createDefaultCP.m');

return;

end

%% nn network paramters

cp.layerMatrix = neuronNumberVector;

%% Kohonen layer

aLayer.number = neuronNumberVector(2);

aLayer.error = [];

aLayer.output = [];

aLayer.neurons = [];

aLayer.analogOutput = [];

aLayer.weights = [];

for ind = 1: aLayer.number

%% create a default neuron

inputsNumber = neuronNumberVector(1);

weights = ones(1,inputsNumber) / sqrt(aLayer.number);

aNeuron.weights = weights;

aNeuron.weightsUpdateNumber = 0;

aNeuron.z = 0;

aNeuron.y = 0;

aLayer.neurons = [aLayer.neurons, aNeuron];

aLayer.weights = [aLayer.weights; weights];

end

cp.kohonen = aLayer;

size(cp.kohonen.weights)

%% Grossberg Layer

aLayer.number = neuronNumberVector(3);

aLayer.error = [];

aLayer.output = [];

aLayer.neurons = [];

aLayer.analogOutput = [];

aLayer.weights = [];

%% create a default layer

for ind = 1: aLayer.number

%% create a default neuron

inputsNumber = neuronNumberVector(2);

weights = zeros(1,inputsNumber);

aNeuron.weights = weights;

aNeuron.weightsUpdateNumber = 0;

aNeuron.z = 0;

aNeuron.y = 0;

aLayer.neurons = [aLayer.neurons, aNeuron];

aLayer.weights = [aLayer.weights; weights];

end

cp.grossberg = aLayer;

% getCPTestingData.m

function testingData = getCPTestingData(gesdata)

testingData(1).input = gesdata;

% getCPTrainingData.m

% This function is used for getting the training data from text files

function [trainingData, classNumber] = getCPTrainingData

A = [0 0 0 1;0 0 1 0;0 0 1 1;0 1 0 0;0 1 0 1];

classID = 0;

for j=1:3:15

classID = classID + 1;

for i = 0:2

filename = strcat('gest',num2str(j+i),'.txt');

imag = getinp(filename);

trainingData(j+i).input =imag;

trainingData(j+i).classID = classID;

trainingData(j+i).output = A(classID,:);

trainingData(j+i).name = num2str(classID);

end

classNumber = classID;

% getinp.m

function inp = getinp(filename)

fid = fopen(filename,'r');

inp = fscanf(fid,'%f');

fclose(fid);

% propagatingCP.m

function [cp, output] = propagatingCP(cp, inputData)

output = [];

if nargin < 2

display('propagatingCP.m needs two parameters');

return;

end

% Propagation of Kohonen Layer

size(inputData)

size(cp.kohonen.weights )

zOut = cp.kohonen.weights * inputData;

display('size inputData');

size(inputData)

[zMax, zMaxInd] = max(zOut);

% Tell me which Neuron in Kohonen Layer fired

output = zMaxInd;

% Uncomment the follwoing code, but has to be used during Network Learning

% yOut = zeros(size(zOut));

% yOut(zMaxInd) = 1;

% display('I am yOut: ');

% yOut

% cp.kohonen.analogOutput = zOut;

% cp.kohonen.output = yOut;

% for kInd =1 : 5%cp.kohonen.number

% cp.kohonen.neurons(kInd).z = zOut(kInd);

% cp.kohonen.neurons(kInd).y = yOut(kInd);

% end

%

% % % propagation of Grossberg Layer

%

% zOut = cp.grossberg.weights * yOut;

% display('I am zOut2');

% size(zOut)

% yOut = zOut;

% display('I am yOut2');

% size(yOut)

% cp.grossberg.analogOutput = zOut;

% cp.grossberg.output = yOut;

% % for gInd =1 : cp.grossberg.number

% % cp.grossberg.neurons(gInd).z = zOut(gInd);

% % cp.grossberg.neurons(gInd).y = yOut(gInd);

% % end

% display('GROSSBERG OUTPUT');

% cp.grossberg.output;

% trainingCP.m

function cp = trainingCP(cp, trainingData )

if nargin < 2

display('trainingCP.m needs at least two parameter');

return;

end

oldclassID=0;

summ=0;

datbasetSize = size(trainingData);

kWeights = [];

gWeights = zeros(cp.grossberg.number,cp.kohonen.number);

for datbasetIndex = 1: datbasetSize(2)

mIn = trainingData(datbasetIndex).input(:);

mOut = trainingData(datbasetIndex).output(:);

mClassID = trainingData(datbasetIndex).classID;

mIn = mIn / sqrt(sum(mIn.*mIn));

%% training the Kohonen Layer

oldweights = cp.kohonen.neurons(mClassID).weights;

weightUpdateNumber = cp.kohonen.neurons(mClassID).weightsUpdateNumber + 1;

if weightUpdateNumber > 1

mIn = ((oldweights * weightUpdateNumber)' + mIn) / weightUpdateNumber;

mIn = mIn / sqrt(sum(mIn .* mIn));

end

cp.kohonen.neurons(mClassID).weights = mIn';

cp.kohonen.neurons(mClassID).weightsUpdateNumber = weightUpdateNumber;

if(weightUpdateNumber == 3)

kWeights = [kWeights; mIn'];

end

%% training the Grossberg Layer

gWeights(: ,mClassID) = mOut;

end

for gInd = 1: cp.grossberg.number

cp.grossberg.neurons(gInd).weights = gWeights(gInd,:);

end

cp.kohonen.weights = kWeights;

cp.grossberg.weights = gWeights;

save cp_kohonen_weights kWeights;

save cp_grossberg_weights gWeights;

Final Project - Robotics: Algorithms and Control ( ECE 452 )