Difference between revisions of "Acrob navigation in maze using neural network (Kundlová, Jurenka)"
From RoboWiki
Line 1: | Line 1: | ||
− | [[ | + | ---- |
+ | Backpropagation v Matlabe | ||
+ | |||
+ | function [J, grad] = nnCostFunction(nn_params, ... | ||
+ | input_layer_size, ... | ||
+ | hidden_layer_size, ... | ||
+ | num_labels, ... | ||
+ | X, y, lambda) | ||
+ | Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ... | ||
+ | hidden_layer_size, (input_layer_size + 1)); | ||
+ | Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ... | ||
+ | num_labels, (hidden_layer_size + 1)); | ||
+ | m = size(X, 1); | ||
+ | J = 0; | ||
+ | Theta1_grad = zeros(size(Theta1)); | ||
+ | Theta2_grad = zeros(size(Theta2)); | ||
+ | X = [ones(m, 1) X]; | ||
+ | a1 = X; | ||
+ | z2 = a1*Theta1'; | ||
+ | a2 = sigmoid(z2); | ||
+ | a2 = [ones(size(a2,1), 1) a2]; | ||
+ | z3 = a2*Theta2'; | ||
+ | a3 = sigmoid(z3); | ||
+ | h = a3; | ||
+ | y1 = zeros(size(y,1), num_labels); | ||
+ | for i=1:size(y) | ||
+ | y1(i, y(i)) = 1; | ||
+ | end | ||
+ | J = 1/m * sum(sum(((-y1).* log(h) - (1-y1).* log(1-h)))); | ||
+ | temp1 = Theta1; | ||
+ | temp1(:,1) = zeros(size(temp1,1),1); | ||
+ | temp2 = Theta2; | ||
+ | temp2(:,1) = zeros(size(temp2,1),1); | ||
+ | J = J + lambda / (2*m) * ( ... | ||
+ | sum(sum(temp1 .*temp1)) + sum(sum(temp2.*temp2)) ... | ||
+ | ); | ||
+ | for t = 1:m | ||
+ | a1 = X(t,:); | ||
+ | z2 = a1*Theta1'; | ||
+ | a2 = sigmoid(z2); | ||
+ | a2 = [ones(size(a2,1), 1) a2]; | ||
+ | z3 = a2*Theta2'; | ||
+ | a3 = sigmoid(z3); | ||
+ | yk = zeros(num_labels,1); | ||
+ | yk(y(t)) = 1; | ||
+ | d3 = a3' - yk; | ||
+ | d2 = (Theta2'*d3).*[ones(1,1); sigmoidGradient(z2')]; | ||
+ | d2 = d2(2:end); | ||
+ | Theta1_grad = Theta1_grad + d2*a1; | ||
+ | Theta2_grad = Theta2_grad + d3*a2; | ||
+ | end | ||
+ | Theta1_grad = Theta1_grad / m; | ||
+ | Theta2_grad = Theta2_grad / m; | ||
+ | Theta1_grad = Theta1_grad + ( (lambda/m) .* [ zeros( hidden_layer_size, 1 ), Theta1( :, 2:end ) ] ); | ||
+ | Theta2_grad = Theta2_grad + ( (lambda/m) .* [ zeros( num_labels, 1 ), Theta2( :, 2:end ) ] ); | ||
+ | grad = [Theta1_grad(:) ; Theta2_grad(:)]; | ||
+ | end | ||
+ | |||
+ | |||
+ | ---- | ||
+ | |||
+ | Java | ||
+ | |||
+ | |||
+ | [http://www.youtube.com/watch?v=RTendHbPHPU Video] | ||
+ | ---- | ||
+ | |||
+ | |||
+ | #include <Servo.h> | ||
+ | Servo LeftServo; | ||
+ | Servo RightServo; | ||
+ | double THETA_1[5][5] = { | ||
+ | { -3.3015, -3.2262, 8.4318, -4.8450, 15.3742}, | ||
+ | { -4.7470, -0.3573, -9.7632, -9.3562, 18.5839}, | ||
+ | { -13.2009, 8.7361, 8.5763, 7.5276, 10.6005}, | ||
+ | { 7.0469, -10.8166, -9.0304, -4.0693, 13.2435}, | ||
+ | { 2.6464, -6.6205, -2.8116, 11.4895, 8.3321} | ||
+ | }; | ||
+ | double THETA_2[5][6] = { | ||
+ | { -12.1840, 25.3169, 27.4735, -23.5952, -0.5421, -2.3738}, | ||
+ | { -14.6229, -2.5615, -26.4061, -0.6488, 27.8756, 1.7607}, | ||
+ | { 12.8366, -16.8083, -3.1984, -14.8165, -10.5194, -16.8826}, | ||
+ | { -17.2910, -4.2473, -10.6967, 8.6676, -24.9129, 25.1679}, | ||
+ | { -12.0564, 3.1205, -3.9859, 21.9216, -4.5046, -25.5430} | ||
+ | }; | ||
+ | char* ACTION_NAMES[]={ | ||
+ | "ERROR", | ||
+ | "FORWARD", | ||
+ | "RIGHT", | ||
+ | "LEFT", | ||
+ | "FORWARD RIGHT", | ||
+ | "FORWARD LEFT" | ||
+ | }; | ||
+ | double LINE_TRESHOLD = 0.5; | ||
+ | #define FW_SPEED 1.0 | ||
+ | #define BIG_ROT_AMOUNT 1.0 | ||
+ | #define SLOWDOWN 1 | ||
+ | #define SMALL_ROT_AMOUNT 1 | ||
+ | #define LEFT_SENSOR 3 | ||
+ | #define RIGHT_SENSOR 2 | ||
+ | #define CENTER_SENSOR 0 | ||
+ | #define FRONT_SENSOR 1 | ||
+ | #define TRESHOLD 500 | ||
+ | double ACTIONS[6][2] = { | ||
+ | {0,0}, | ||
+ | {FW_SPEED, 0}, | ||
+ | {0, -BIG_ROT_AMOUNT }, | ||
+ | {0, BIG_ROT_AMOUNT }, | ||
+ | {/*FW_SPEED / SLOWDOWN*/0, -SMALL_ROT_AMOUNT}, | ||
+ | {/*FW_SPEED / SLOWDOWN*/0, SMALL_ROT_AMOUNT} | ||
+ | } ; | ||
+ | #define INPUT_SIZE 4 | ||
+ | #define Euler 2.71828182845904523536 | ||
+ | #define THETA_1_ROWS 5 | ||
+ | #define THETA_1_COLUMNS 5 | ||
+ | #define THETA_2_ROWS 5 | ||
+ | #define THETA_2_COLUMNS 6 | ||
+ | int REPEAT_1[INPUT_SIZE] = {0,0,0,0}; | ||
+ | int REPEAT_2[INPUT_SIZE] = {1,1,1,0}; | ||
+ | int last = 1; | ||
+ | boolean compareArrays(int a[], int b[]){ | ||
+ | for(int i =0 ; i < INPUT_SIZE; i++){ | ||
+ | if(a[i] != b[i]){ | ||
+ | return false; | ||
+ | } | ||
+ | } | ||
+ | return true; | ||
+ | } | ||
+ | void transpose(double * matrix, double * result , int rr, int cc){ | ||
+ | for(int r =0 ; r < rr; r++){ | ||
+ | for(int c =0 ; c < cc; c++){ | ||
+ | result[c * rr + r] = matrix[r * cc + c]; | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | int feed(int input[]) { | ||
+ | if (last != 1 && ( compareArrays(input, REPEAT_1) || compareArrays (input, REPEAT_2) )) { | ||
+ | return last; | ||
+ | } | ||
+ | double x[1][INPUT_SIZE]; | ||
+ | for (int i = 0; i < INPUT_SIZE; i++) { | ||
+ | x[0][i] = input[i]; | ||
+ | } | ||
+ | int m = 1; | ||
+ | double ones[m][1]; | ||
+ | for (int i = 0; i < m; i++) { | ||
+ | ones[i][0] = 1; | ||
+ | } | ||
+ | double t1t[THETA_1_COLUMNS][THETA_1_ROWS]; | ||
+ | transpose((double*)THETA_1, (double*)t1t , THETA_1_ROWS, THETA_1_COLUMNS); | ||
+ | double t2t[THETA_2_COLUMNS][THETA_2_ROWS]; | ||
+ | transpose((double*)THETA_2, (double*)t2t, THETA_2_ROWS, THETA_2_COLUMNS); | ||
+ | double a0[m][INPUT_SIZE+1]; | ||
+ | int params[6]; | ||
+ | params[0] = m; | ||
+ | params[1] = INPUT_SIZE+1; | ||
+ | params[2] = m; | ||
+ | params[3] = 1; | ||
+ | params[4] = 0; | ||
+ | params[5] = 0; | ||
+ | combineMatrix((double*)ones, (double*)a0, params); | ||
+ | params[2] = m; | ||
+ | params[3] = INPUT_SIZE; | ||
+ | params[4] = 0; | ||
+ | params[5] = 1; | ||
+ | combineMatrix((double*)x, (double*)a0, params); | ||
+ | double h1[m][THETA_1_ROWS]; | ||
+ | zeroMatrix((double*)h1, m, THETA_1_ROWS); | ||
+ | int mparams[4]; | ||
+ | mparams[0] = m; | ||
+ | mparams[1] = INPUT_SIZE + 1; | ||
+ | mparams[2] = THETA_1_COLUMNS; | ||
+ | mparams[3] = THETA_1_ROWS; | ||
+ | multiply((double*)a0, (double*)t1t, (double*)h1, mparams); | ||
+ | sigmoid((double*)h1,(double*)h1 , m , THETA_1_ROWS); | ||
+ | double a1[m][THETA_1_ROWS+1]; | ||
+ | params[0] = m; | ||
+ | params[1] = THETA_1_ROWS+1; | ||
+ | params[2] = m; | ||
+ | params[3] = 1; | ||
+ | params[4] = 0; | ||
+ | params[5] = 0; | ||
+ | combineMatrix((double*)ones, (double*)a1, params); | ||
+ | params[2] = m; | ||
+ | params[3] = THETA_1_ROWS; | ||
+ | params[4] = 0; | ||
+ | params[5] = 1; | ||
+ | combineMatrix((double*)h1, (double*)a1, params); | ||
+ | double h2[m][THETA_2_COLUMNS]; | ||
+ | zeroMatrix((double*)h2, m, THETA_2_ROWS); | ||
+ | mparams[0] = m; | ||
+ | mparams[1] = THETA_1_ROWS + 1; | ||
+ | mparams[2] = THETA_2_COLUMNS; | ||
+ | mparams[3] = THETA_2_ROWS; | ||
+ | multiply((double*)a1, (double*)t2t, (double*)h2, mparams); | ||
+ | sigmoid((double*)h2,(double*)h2 , m , THETA_2_ROWS); | ||
+ | int actionIndex = maxValue((double*)h2, THETA_2_ROWS * m) + 1; | ||
+ | last = actionIndex; | ||
+ | return actionIndex; | ||
+ | } | ||
+ | double sigmoid(double * matrix, double * result, int rr, int cc){ | ||
+ | for(int r =0 ; r < rr; r++){ | ||
+ | for(int c =0 ; c < cc; c++){ | ||
+ | result[r*cc + c] = 1.0/(1.0+exp(-matrix[r*cc + c])); | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | void copyMatrix(double * matrix, double * result, int rr, int cc){ | ||
+ | int param[6]; | ||
+ | param[0] = rr; | ||
+ | param[1] = cc; | ||
+ | param[2] = rr; | ||
+ | param[3] = cc; | ||
+ | param[4] = 0; | ||
+ | param[5] = 0; | ||
+ | combineMatrix(matrix, result, param); | ||
+ | } | ||
+ | void combineMatrix(double * matrix, double * result, int param[6]){ | ||
+ | int lr = param[0]; | ||
+ | int lc = param[1]; | ||
+ | int sr = param[2]; | ||
+ | int sc = param[3]; | ||
+ | int ro = param[4]; | ||
+ | int co = param[5]; | ||
+ | for(int r =0 ; r < sr; r++){ | ||
+ | for(int c =0 ; c < sc; c++){ | ||
+ | result[(r + ro ) * lc + co + c] = matrix[r * sc + c]; | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | int maxValue(double* A, int length){ | ||
+ | int mValue = 0; | ||
+ | for(int i=0; i<length; i++){ | ||
+ | if(A[mValue] <= A[i]){ | ||
+ | mValue = i; | ||
+ | } | ||
+ | } | ||
+ | return mValue; | ||
+ | } | ||
+ | void multiply(double * A, double * B, double * C, int dim[4]) { | ||
+ | /*int mA = A.length; | ||
+ | int nA = A[0].length; | ||
+ | int mB = B.length; | ||
+ | int nB = A[0].length;*/ | ||
+ | int mA = dim[0]; | ||
+ | int nA = dim[1]; | ||
+ | int mB = dim[2]; | ||
+ | int nB = dim[3]; | ||
+ | if (nA != mB){ | ||
+ | Serial.print("Multiplication error "); | ||
+ | Serial.print(nA); | ||
+ | Serial.print(" "); | ||
+ | Serial.println(mB); | ||
+ | } | ||
+ | for (int i = 0; i < mA; i++){ | ||
+ | for (int j = 0; j < nB; j++){ | ||
+ | for (int k = 0; k < nA; k++){ | ||
+ | C[i*nB+j] += (A[i*nA+k] * B[k*nB+j]); | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | void printMatrix(double * matrix, int rr, int cc){ | ||
+ | for(int r =0 ; r < rr; r++){ | ||
+ | for(int c =0 ; c < cc; c++){ | ||
+ | Serial.print(matrix[r*cc+c]); | ||
+ | Serial.print(" "); | ||
+ | } | ||
+ | Serial.println(); | ||
+ | } | ||
+ | Serial.println(); | ||
+ | } | ||
+ | void zeroMatrix(double * matrix, int rr, int cc){ | ||
+ | for(int r =0 ; r < rr; r++){ | ||
+ | for(int c =0 ; c < cc; c++){ | ||
+ | matrix[r*cc+c] = 0; | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | void setup() { | ||
+ | Serial.begin(9600); | ||
+ | int input[4]; | ||
+ | input[0] = 0; | ||
+ | input[1] = 0; | ||
+ | input[2] = 0; | ||
+ | input[3] = 0; | ||
+ | int actionIndex = feed(input); | ||
+ | Serial.println(actionIndex); | ||
+ | LeftServo.attach(9); | ||
+ | RightServo.attach(10); | ||
+ | } | ||
+ | int convert(long val){ | ||
+ | if(val >= TRESHOLD){ | ||
+ | return 1; | ||
+ | } | ||
+ | return 0; | ||
+ | } | ||
+ | void fw(double left, double right){ | ||
+ | double scale = 1; | ||
+ | int lAmmount = 90 - round(left*scale); | ||
+ | int rAmmount = 90 + round(right*scale); | ||
+ | Serial.print("move "); | ||
+ | Serial.print(lAmmount); | ||
+ | Serial.print(" "); | ||
+ | Serial.println(rAmmount); | ||
+ | LeftServo.write(lAmmount); | ||
+ | RightServo.write(rAmmount); | ||
+ | } | ||
+ | void loop() { | ||
+ | long leftVal = analogRead(LEFT_SENSOR); | ||
+ | long rightVal = analogRead(RIGHT_SENSOR); | ||
+ | long centerVal = analogRead(CENTER_SENSOR); | ||
+ | long frontVal = analogRead(FRONT_SENSOR); | ||
+ | Serial.print("left: "); | ||
+ | Serial.print(leftVal, DEC); | ||
+ | Serial.print(" right: "); | ||
+ | Serial.print(rightVal, DEC); | ||
+ | Serial.print(" center: "); | ||
+ | Serial.print(centerVal, DEC); | ||
+ | Serial.print(" front: "); | ||
+ | Serial.println(frontVal, DEC); | ||
+ | int input[4]; | ||
+ | input[0] = convert(leftVal); | ||
+ | input[1] = convert(centerVal); | ||
+ | input[2] = convert(rightVal); | ||
+ | input[3] = convert(frontVal); | ||
+ | int action = feed(input); | ||
+ | Serial.println(ACTION_NAMES[action]); | ||
+ | double * move = ACTIONS[action]; | ||
+ | double left = move[0] + move[1]; | ||
+ | double right = move[0] - move[1]; | ||
+ | fw(left, right); | ||
+ | delay(1000); | ||
+ | } | ||
+ | |||
+ | |||
+ | [http://www.youtube.com/watch?v=mn_QWL0fBuU Video] | ||
+ | |||
+ | ---- |
Revision as of 16:14, 12 June 2013
Backpropagation v Matlabe
function [J, grad] = nnCostFunction(nn_params, ... input_layer_size, ... hidden_layer_size, ... num_labels, ... X, y, lambda) Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ... hidden_layer_size, (input_layer_size + 1)); Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ... num_labels, (hidden_layer_size + 1)); m = size(X, 1); J = 0; Theta1_grad = zeros(size(Theta1)); Theta2_grad = zeros(size(Theta2)); X = [ones(m, 1) X]; a1 = X; z2 = a1*Theta1'; a2 = sigmoid(z2); a2 = [ones(size(a2,1), 1) a2]; z3 = a2*Theta2'; a3 = sigmoid(z3); h = a3; y1 = zeros(size(y,1), num_labels); for i=1:size(y) y1(i, y(i)) = 1; end J = 1/m * sum(sum(((-y1).* log(h) - (1-y1).* log(1-h)))); temp1 = Theta1; temp1(:,1) = zeros(size(temp1,1),1); temp2 = Theta2; temp2(:,1) = zeros(size(temp2,1),1); J = J + lambda / (2*m) * ( ... sum(sum(temp1 .*temp1)) + sum(sum(temp2.*temp2)) ... ); for t = 1:m a1 = X(t,:); z2 = a1*Theta1'; a2 = sigmoid(z2); a2 = [ones(size(a2,1), 1) a2]; z3 = a2*Theta2'; a3 = sigmoid(z3); yk = zeros(num_labels,1); yk(y(t)) = 1; d3 = a3' - yk; d2 = (Theta2'*d3).*[ones(1,1); sigmoidGradient(z2')]; d2 = d2(2:end); Theta1_grad = Theta1_grad + d2*a1; Theta2_grad = Theta2_grad + d3*a2; end Theta1_grad = Theta1_grad / m; Theta2_grad = Theta2_grad / m; Theta1_grad = Theta1_grad + ( (lambda/m) .* [ zeros( hidden_layer_size, 1 ), Theta1( :, 2:end ) ] ); Theta2_grad = Theta2_grad + ( (lambda/m) .* [ zeros( num_labels, 1 ), Theta2( :, 2:end ) ] ); grad = [Theta1_grad(:) ; Theta2_grad(:)]; end
Java
#include <Servo.h> Servo LeftServo; Servo RightServo; double THETA_1[5][5] = { { -3.3015, -3.2262, 8.4318, -4.8450, 15.3742}, { -4.7470, -0.3573, -9.7632, -9.3562, 18.5839}, { -13.2009, 8.7361, 8.5763, 7.5276, 10.6005}, { 7.0469, -10.8166, -9.0304, -4.0693, 13.2435}, { 2.6464, -6.6205, -2.8116, 11.4895, 8.3321} }; double THETA_2[5][6] = { { -12.1840, 25.3169, 27.4735, -23.5952, -0.5421, -2.3738}, { -14.6229, -2.5615, -26.4061, -0.6488, 27.8756, 1.7607}, { 12.8366, -16.8083, -3.1984, -14.8165, -10.5194, -16.8826}, { -17.2910, -4.2473, -10.6967, 8.6676, -24.9129, 25.1679}, { -12.0564, 3.1205, -3.9859, 21.9216, -4.5046, -25.5430} }; char* ACTION_NAMES[]={ "ERROR", "FORWARD", "RIGHT", "LEFT", "FORWARD RIGHT", "FORWARD LEFT" }; double LINE_TRESHOLD = 0.5; #define FW_SPEED 1.0 #define BIG_ROT_AMOUNT 1.0 #define SLOWDOWN 1 #define SMALL_ROT_AMOUNT 1 #define LEFT_SENSOR 3 #define RIGHT_SENSOR 2 #define CENTER_SENSOR 0 #define FRONT_SENSOR 1 #define TRESHOLD 500 double ACTIONS[6][2] = { {0,0}, {FW_SPEED, 0}, {0, -BIG_ROT_AMOUNT }, {0, BIG_ROT_AMOUNT }, {/*FW_SPEED / SLOWDOWN*/0, -SMALL_ROT_AMOUNT}, {/*FW_SPEED / SLOWDOWN*/0, SMALL_ROT_AMOUNT} } ; #define INPUT_SIZE 4 #define Euler 2.71828182845904523536 #define THETA_1_ROWS 5 #define THETA_1_COLUMNS 5 #define THETA_2_ROWS 5 #define THETA_2_COLUMNS 6 int REPEAT_1[INPUT_SIZE] = {0,0,0,0}; int REPEAT_2[INPUT_SIZE] = {1,1,1,0}; int last = 1; boolean compareArrays(int a[], int b[]){ for(int i =0 ; i < INPUT_SIZE; i++){ if(a[i] != b[i]){ return false; } } return true; } void transpose(double * matrix, double * result , int rr, int cc){ for(int r =0 ; r < rr; r++){ for(int c =0 ; c < cc; c++){ result[c * rr + r] = matrix[r * cc + c]; } } } int feed(int input[]) { if (last != 1 && ( compareArrays(input, REPEAT_1) || compareArrays (input, REPEAT_2) )) { return last; } double x[1][INPUT_SIZE]; for (int i = 0; i < INPUT_SIZE; i++) { x[0][i] = input[i]; } int m = 1; double ones[m][1]; for (int i = 0; i < m; i++) { ones[i][0] = 1; } double t1t[THETA_1_COLUMNS][THETA_1_ROWS]; transpose((double*)THETA_1, (double*)t1t , THETA_1_ROWS, THETA_1_COLUMNS); double t2t[THETA_2_COLUMNS][THETA_2_ROWS]; transpose((double*)THETA_2, (double*)t2t, THETA_2_ROWS, THETA_2_COLUMNS); double a0[m][INPUT_SIZE+1]; int params[6]; params[0] = m; params[1] = INPUT_SIZE+1; params[2] = m; params[3] = 1; params[4] = 0; params[5] = 0; combineMatrix((double*)ones, (double*)a0, params); params[2] = m; params[3] = INPUT_SIZE; params[4] = 0; params[5] = 1; combineMatrix((double*)x, (double*)a0, params); double h1[m][THETA_1_ROWS]; zeroMatrix((double*)h1, m, THETA_1_ROWS); int mparams[4]; mparams[0] = m; mparams[1] = INPUT_SIZE + 1; mparams[2] = THETA_1_COLUMNS; mparams[3] = THETA_1_ROWS; multiply((double*)a0, (double*)t1t, (double*)h1, mparams); sigmoid((double*)h1,(double*)h1 , m , THETA_1_ROWS); double a1[m][THETA_1_ROWS+1]; params[0] = m; params[1] = THETA_1_ROWS+1; params[2] = m; params[3] = 1; params[4] = 0; params[5] = 0; combineMatrix((double*)ones, (double*)a1, params); params[2] = m; params[3] = THETA_1_ROWS; params[4] = 0; params[5] = 1; combineMatrix((double*)h1, (double*)a1, params); double h2[m][THETA_2_COLUMNS]; zeroMatrix((double*)h2, m, THETA_2_ROWS); mparams[0] = m; mparams[1] = THETA_1_ROWS + 1; mparams[2] = THETA_2_COLUMNS; mparams[3] = THETA_2_ROWS; multiply((double*)a1, (double*)t2t, (double*)h2, mparams); sigmoid((double*)h2,(double*)h2 , m , THETA_2_ROWS); int actionIndex = maxValue((double*)h2, THETA_2_ROWS * m) + 1; last = actionIndex; return actionIndex; } double sigmoid(double * matrix, double * result, int rr, int cc){ for(int r =0 ; r < rr; r++){ for(int c =0 ; c < cc; c++){ result[r*cc + c] = 1.0/(1.0+exp(-matrix[r*cc + c])); } } } void copyMatrix(double * matrix, double * result, int rr, int cc){ int param[6]; param[0] = rr; param[1] = cc; param[2] = rr; param[3] = cc; param[4] = 0; param[5] = 0; combineMatrix(matrix, result, param); } void combineMatrix(double * matrix, double * result, int param[6]){ int lr = param[0]; int lc = param[1]; int sr = param[2]; int sc = param[3]; int ro = param[4]; int co = param[5]; for(int r =0 ; r < sr; r++){ for(int c =0 ; c < sc; c++){ result[(r + ro ) * lc + co + c] = matrix[r * sc + c]; } } } int maxValue(double* A, int length){ int mValue = 0; for(int i=0; i<length; i++){ if(A[mValue] <= A[i]){ mValue = i; } } return mValue; } void multiply(double * A, double * B, double * C, int dim[4]) { /*int mA = A.length; int nA = A[0].length; int mB = B.length; int nB = A[0].length;*/ int mA = dim[0]; int nA = dim[1]; int mB = dim[2]; int nB = dim[3]; if (nA != mB){ Serial.print("Multiplication error "); Serial.print(nA); Serial.print(" "); Serial.println(mB); } for (int i = 0; i < mA; i++){ for (int j = 0; j < nB; j++){ for (int k = 0; k < nA; k++){ C[i*nB+j] += (A[i*nA+k] * B[k*nB+j]); } } } } void printMatrix(double * matrix, int rr, int cc){ for(int r =0 ; r < rr; r++){ for(int c =0 ; c < cc; c++){ Serial.print(matrix[r*cc+c]); Serial.print(" "); } Serial.println(); } Serial.println(); } void zeroMatrix(double * matrix, int rr, int cc){ for(int r =0 ; r < rr; r++){ for(int c =0 ; c < cc; c++){ matrix[r*cc+c] = 0; } } } void setup() { Serial.begin(9600); int input[4]; input[0] = 0; input[1] = 0; input[2] = 0; input[3] = 0; int actionIndex = feed(input); Serial.println(actionIndex); LeftServo.attach(9); RightServo.attach(10); } int convert(long val){ if(val >= TRESHOLD){ return 1; } return 0; } void fw(double left, double right){ double scale = 1; int lAmmount = 90 - round(left*scale); int rAmmount = 90 + round(right*scale); Serial.print("move "); Serial.print(lAmmount); Serial.print(" "); Serial.println(rAmmount); LeftServo.write(lAmmount); RightServo.write(rAmmount); } void loop() { long leftVal = analogRead(LEFT_SENSOR); long rightVal = analogRead(RIGHT_SENSOR); long centerVal = analogRead(CENTER_SENSOR); long frontVal = analogRead(FRONT_SENSOR); Serial.print("left: "); Serial.print(leftVal, DEC); Serial.print(" right: "); Serial.print(rightVal, DEC); Serial.print(" center: "); Serial.print(centerVal, DEC); Serial.print(" front: "); Serial.println(frontVal, DEC); int input[4]; input[0] = convert(leftVal); input[1] = convert(centerVal); input[2] = convert(rightVal); input[3] = convert(frontVal); int action = feed(input); Serial.println(ACTION_NAMES[action]); double * move = ACTIONS[action]; double left = move[0] + move[1]; double right = move[0] - move[1]; fw(left, right); delay(1000); }