Close

Step 6. Neural Networks

A project log for UV Sanitizing Autonomous Robot

Cost-effective robotic solution for surface sanitization in home

guillermo-perez-guillenGuillermo Perez Guillen 08/25/2021 at 18:410 Comments

In this project we will create a neural network with Python and copy its weights to a network with forward propagation on the Arduino UNO board, and that will allow the Autonomous Robot to drive alone and without hitting the walls.

For this exercise we will make the neural network have 4 outputs: two for each motor pair, since to the L298N driver we will connect 2 digital outputs of the board for each car motor pair (the two motors on the left are electrically linked, the same case with the two motors on the right.). In addition the outputs will be between 0 and 1 (depolarize or polarize the motor).

Neural Networks

                              Neural Networks

We will have seven inputs:

The inputs are assigned with the following logic:

Here we see the changes in the table below:

Inputs, Outputs and Actions of the gearmotors                              Inputs, Outputs and Actions of the Gearmotors

To create our neural network, we will use this code developed with Python 3.7.3: neural-network.py

import numpy as np

# We create the class 
    def __init__(self, layers, activation='tanh'):
        if activation == 'sigmoid':
            self.activation = sigmoid
            self.activation_prime = sigmoid_derivada
        elif activation == 'tanh':
            self.activation = tanh
            self.activation_prime = tanh_derivada

        # Assign random values to input layer and hidden layer
        for i in range(1, len(layers) - 1):
            r = 2*np.random.random((layers[i-1] + 1, layers[i] + 1)) -1
            self.weights.append(r)
        # Assigned random to output layer
        r = 2*np.random.random( (layers[i] + 1, layers[i+1])) - 1
        self.weights.append(r)

    def fit(self, X, y, learning_rate=0.2, epochs=100000):
        # I add column of ones to the X inputs. With this we add the Bias unit to the input layer
        ones = np.atleast_2d(np.ones(X.shape[0]))
        X = np.concatenate((ones.T, X), axis=1)
        
        for k in range(epochs):
            i = np.random.randint(X.shape[0])
            a = [X[i]]
            for l in range(len(self.weights)):
                    dot_value = np.dot(a[l], self.weights[l])
                    activation = self.activation(dot_value)
                    a.append(activation)
            #Calculate the difference in the output layer and the value obtained
            error = y[i] - a[-1]
            deltas = [error * self.activation_prime(a[-1])]
            
            # We start in the second layer until the last one (A layer before the output one)
            for l in range(len(a) - 2, 0, -1): 
                deltas.append(deltas[-1].dot(self.weights[l].T)*self.activation_prime(a[l]))
            self.deltas.append(deltas)

            # Reverse
            deltas.reverse()

            # Backpropagation
            # 1. Multiply the output delta with the input activations to obtain the weight gradient.             
            # 2. Updated the weight by subtracting a percentage of the gradient
            for i in range(len(self.weights)):
                layer = np.atleast_2d(a[i])
                delta = np.atleast_2d(deltas[i])
                self.weights[i] += learning_rate * layer.T.dot(delta)

    def predict(self, x): 
        ones = np.atleast_2d(np.ones(x.shape[0]))
        a = np.concatenate((np.ones(1).T, np.array(x)), axis=0)
        for l in range(0, len(self.weights)):
            a = self.activation(np.dot(a, self.weights[l]))
        return a

    def print_weights(self):
        print("LIST OF CONNECTION WEIGHTS")
        for i in range(len(self.weights)):
            print(self.weights[i])

    def get_weights(self):
        return self.weights
    
    def get_deltas(self):
        return self.deltas

# When creating the network, we can choose between using the sigmoid or tanh function
def sigmoid(x):
    return 1.0/(1.0 + np.exp(-x))

def sigmoid_derivada(x):
    return sigmoid(x)*(1.0-sigmoid(x))

def tanh_derivada(x):
    return 1.0 - x**2

########## CAR NETWORK

nn = NeuralNetwork([6,3,4],activation ='tanh')
X = np.array([[0,0,0,0,0,0],   
              [0,0,0,0,0,1],   
              [0,0,0,0,1,0],   
              [0,0,0,0,1,1],   
              [0,0,0,1,0,0],   
              [0,0,0,1,0,1],   
              [0,0,0,1,1,0],   
              [0,0,0,1,1,1],   
              [0,0,1,0,0,0],  
              [0,0,1,0,0,1],   
              [0,0,1,0,1,1],   
              [0,0,1,1,0,0],   
              [0,0,1,1,0,1],   
              [0,0,1,1,1,1],   
              [0,1,0,0,0,0],   
              [0,1,0,0,0,1],   
              [0,1,0,0,1,0],   
              [0,1,0,1,0,0],   
              [0,1,0,1,0,1],   
              [0,1,0,1,1,0],   
              [0,1,1,0,0,0],   
              [0,1,1,0,1,0],   
              [0,1,1,1,0,0],   
              [0,1,1,1,1,0],   
              [1,0,0,0,0,0],   
              [1,0,0,0,0,1],   
              [1,0,0,0,1,0],   
              [1,0,0,0,1,1],   
              [1,0,0,1,0,0],   
              [1,0,0,1,0,1],   
              [1,0,0,1,1,0],   
              [1,0,0,1,1,1],   
              [1,0,1,0,0,0],   
              [1,0,1,0,0,1],   
              [1,0,1,0,1,1],   
              [1,0,1,1,0,0],   
              [1,0,1,1,0,1],   
              [1,0,1,1,1,1],   
              [1,1,0,0,0,0],   
              [1,1,0,0,0,1],   
              [1,1,0,0,1,0],   
              [1,1,0,1,0,0],   
              [1,1,0,1,0,1],   
              [1,1,0,1,1,0],   
              [1,1,1,0,0,0],   
              [1,1,1,0,1,0],   
              [1,1,1,1,0,0],   
              [1,1,1,1,1,0],   
             ])
# the outputs correspond to starting (or not) the motors
y = np.array([[0,0,0,0], # stop
              [0,0,0,0], # stop 
              [0,0,0,0], # stop
              [0,0,0,0], # stop
              [0,0,0,0], # stop 
              [0,0,0,0], # stop
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop
              [0,0,0,0], # stop 
              [0,0,0,0], # stop              
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop
              [0,0,0,0], # stop
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop 
              [0,0,0,0], # stop                
              [1,0,1,0], # forward 
              [1,0,1,0], # forward 
              [0,1,1,0], # turn-left
              [0,1,1,0], # turn-left 
              [0,1,0,1], # back
              [0,1,1,0], # turn-left 
              [0,1,1,0], # turn-left 
              [0,1,1,0], # turn-left 
              [1,0,0,1], # turn-right 
              [0,1,1,0], # turn-left
              [0,1,1,0], # turn-left
              [1,0,0,1], # turn-right
              [0,1,1,0], # turn-left              
              [1,0,0,1], # turn-right 
              [1,0,1,0], # forward 
              [1,0,1,0], # forward
              [1,0,0,1], # turn-right 
              [1,0,0,1], # turn-right 
              [0,1,0,1], # back 
              [1,0,0,1], # turn-right 
              [1,0,0,1], # turn-right
              [1,0,0,1], # turn-right
              [1,0,0,1], # turn-right              
              [1,0,0,1], # turn-right            
             ])
nn.fit(X, y, learning_rate=0.03,epochs=550001)
 
def valNN(x):
return (int)(abs(round(x)))
 
index=0
for e in X:
    prediccion = nn.predict(e)
    print("X:",e,"expected:",y[index],"obtained:", valNN(prediccion[0]),valNN(prediccion[1]),valNN(prediccion[2]),valNN(prediccion[3]))
    index=index+1

We can see the next data:

Expected and obtained values with this code

                              Expected and obtained values with this code

The code we add to see the graph, as it trains after 550,000 iterations is as follows: graphic.py

########## WE GRAPH THE COST FUNCTION
import matplotlib.pyplot as plt
deltas = nn.get_deltas()
valores=[]
index=0
index=index+1
plt.plot(range(len(valores)), valores, color='b')
plt.ylim([0, 1])
plt.ylabel('Cost')
plt.xlabel('Epochs')
plt.tight_layout()
plt.show()

The cost graph decreases after 500,000 iterationsAnd now we can see the weights obtained from the connections, and which will be the ones we will use in the Arduino code: generate-arduino-code.py

########## WE GENERATE THE ARDUINO CODE
def to_str(name, W):
    s = str(W.tolist()).replace('[', '{').replace(']', '}')
    return 'float '+name+'['+str(W.shape[0])+']['+str(W.shape[1])+'] = ' + s + ';'

# We get the weights trained to be able to use them in the arduino code

print('// Replace these lines in your arduino code:')
print('// float HiddenWeights ...')
print('// float OutputWeights ...')
print('// With trained weights.')
print('\n')
print(to_str('HiddenWeights', pesos[0]))
print(to_str('OutputWeights', pesos[1]))

Hidden weights and Output weights of our Neural NetworkThe Arduino code with the configuration of the neural network is loaded on the Arduino UNO board: autonomous-robot.ino

// AUTHOR: GUILLERMO PEREZ GUILLEN

#define ENA 3
#define ENB 5
#define IN1 8
#define IN2 9
#define IN3 10
#define IN4 11

/******************************************************************
   NETWORK CONFIGURATION
******************************************************************/
const int ESP32_pin_1= 6; // ESP32 input pin 1 - starting
const int ESP32_pin_2 = 7; // ESP32 input pin 2 - SRF04
const int ESP32_pin_3 = 12; // ESP32 input pin 3 - SRF05
const int InputNodes = 7; // includes BIAS neuron
const int HiddenNodes = 4; //includes BIAS neuron
const int OutputNodes = 4;
int i, j;
float HiddenWeights[7][4] = {{-4.618963658666277, 4.3001137618883325, 7.338055706191847, 2.7355309007172375}, {2.599633307446623, -7.649705724376986, -14.69443684121685, -3.65366992422193}, {-0.7777191662679982, 1.9860139431844053, 5.914809078303235, 0.03170277380327093}, {-2.309653145069323, 6.8379997039119775, 8.892299055796917, 0.6046238076393062}, {1.3276547120093833, 5.085574619860947, 2.384944264717347, 0.05753178068519734}, {-2.7696264005599858, 6.797226565794283, 3.5374247269984713, 0.5475825968169957}, {0.8118152131237218, -1.9324229493484606, -5.264294920291424, -0.036800281071245555}};
float OutputWeights[4][4] = {{-1.6342640637903814, 0.006920937706630823, -5.179205882976105, -0.40268984302793936}, {-1.0162353344988182, 1.3405072244655225, -4.241619375014734, 0.6682851389512594}, {1.3692632942485174, -1.3884291338648505, -0.9245235380688354, 2.246128813012694}, {-1.9802299382328057, 0.06512857708456388, -0.030302930346753857, -3.314024844617794}};

int error=0;
int dif,difAnt=0;
const float Kp=0.5671;
const float Kd=110.1;

void setup() {    
    Serial.begin(9600);
    pinMode(A0, INPUT); //left sensor
    pinMode(A1, INPUT); //center sensor
    pinMode(A3, INPUT); //right sensor 
    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);
    pinMode(ENA, OUTPUT);
    pinMode(ENB, OUTPUT);
  pinMode(ESP32_pin_1, INPUT); 
  pinMode(ESP32_pin_2, INPUT);
  pinMode(ESP32_pin_3, INPUT);  
} 

void loop()
{

double TestInput[] = {0, 0, 0};
double input1=0,input2=0,input3=0,input4=0,input5=0,input6=0;
    
float volts0 =  analogRead(A0)*0.0048828125;  // value from sensor * (5/1024) 
float volts1 =  analogRead(A1)*0.0048828125;  // value from sensor * (5/1024) 
float volts2 =  analogRead(A3)*0.0048828125;  // value from sensor * (5/1024)

dif = analogRead(A3) - analogRead(A0);    // PID CONTROLLER
error = floor(Kp*(dif)+Kd*(difAnt-dif));    // PID CONTROLLER
difAnt=dif;    // PID CONTROLLER
int d0 = constrain(150 - error, 0, 150);//left speed - PID CONTROLLER

float ir_sensor_left =  6*pow(volts0, -1); // worked out from datasheet graph //GP2Y0A51SK0F - 2 a 15 cm
float ir_sensor_center = 12.4*pow(volts1, -1); // worked out from datasheet graph //GP2Y0A41SK0F - 4 a 30 cm
float ir_sensor_right = 5.2*pow(volts2, -1); // worked out from datasheet graph //GP2Y0A51SK0F - 2 a 15 cm

if (ir_sensor_left<15){input2=1;} // IR SENSOR LEFT
else {input2=0;}

if(digitalRead(ESP32_pin_2) == HIGH){input3=1;} // RF SENSOR LEFT
else {input3=0;}
    
if (ir_sensor_center<30){input4=1;}  // IR SENSOR CENTER
else {input4=0;}

if(digitalRead(ESP32_pin_3) == HIGH){input5=1;} // RF SENSOR RIGHT
else {input5=0;}

if (ir_sensor_right<15){input6=1;} // IR SENSOR RIGHT
else {input6=0;}

/******************************************************************
    WE CALL THE FEEDFORWARD NETWORK WITH THE INPUTS
******************************************************************/

  Serial.print("Input1:");
  Serial.println(input1);
  Serial.print("Input2:");
  Serial.println(input2);
  Serial.print("Input3:");
  Serial.println(input3);  
  Serial.print("Input4:");
  Serial.println(input4);
  Serial.print("Input5:");
  Serial.println(input5);
  Serial.print("Input6:");
  Serial.println(input6); 
  Serial.println("   ");   
  
//THESE ARE THE THREE INPUTS WITH VALUES OF 0 TO 1 ********************
  TestInput[0] = 1.0;//BIAS UNIT
  TestInput[1] = input1;
  TestInput[2] = input2;
  TestInput[3] = input3;  
  TestInput[4] = input4;
  TestInput[5] = input5;
  TestInput[6] = input6;  

// THIS FUNCTION IS TO GET THE OUTPUTS **********************************
  InputToOutput(TestInput[0], TestInput[1], TestInput[2], TestInput[3], TestInput[4], TestInput[5], TestInput[6]); //INPUT to ANN to obtain OUTPUT

  int out1 = round(abs(Output[0]));
  int out2 = round(abs(Output[1]));
  int out3 = round(abs(Output[2]));
  int out4 = round(abs(Output[3]));
  Serial.print("Output1:");
  Serial.println(out1);
  Serial.print("Output2:");
  Serial.println(out2);
  Serial.print("Output3:");
  Serial.println(out3);

/******************************************************************
    DRIVE MOTORS WITH THE NETWORK OUTPUT
******************************************************************/
  analogWrite(ENA, d0);
  analogWrite(ENB, d1);
  digitalWrite(IN1, out1 * HIGH); 
  digitalWrite(IN2, out2 * HIGH); 
  digitalWrite(IN3, out3 * HIGH);
  digitalWrite(IN4, out4 * HIGH);
  delay(20);
}

void InputToOutput(double In1, double In2, double In3, double In4, double In5, double In6, double In7)
{
  double TestInput[] = {0, 0, 0, 0, 0, 0, 0, 0};
  TestInput[0] = In1;
  TestInput[1] = In2;
  TestInput[2] = In3;
  TestInput[3] = In4;
  TestInput[4] = In5;
  TestInput[5] = In6;
  TestInput[6] = In7;    

/******************************************************************
    CALCULATE ACTIVITIES IN HIDDEN LAYERS
******************************************************************/

  for ( i = 0 ; i < HiddenNodes ; i++ ) {    // We go through the four columns of the hidden weights
    Accum = 0;
    for ( j = 0 ; j < InputNodes ; j++ ) {    // Three values of the entry line and each column of hidden weights
      Accum += TestInput[j] * HiddenWeights[j][i] ;
    }
    Hidden[i] = tanh(Accum) ; // We obtain a matrix of a line with four values
  }

Discussions