Neural Network for Robot

So, I built a little obstacle avoidance robot, so far it's working fine. But I would like to make it work with a Neural Network, searching I found this code:

#include <Servo.h>  //servo library
Servo myservo;      // create servo object to control servo
 
int Echo = A4;  
int Trig = A5; 
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11

/******************************************************************
   Network Configuration
 ******************************************************************/
const int InputNodes = 3; // includes BIAS neuron
const int HiddenNodes = 4; //includes BIAS neuron
const int OutputNodes = 4;
int i, j;
double Accum;
double Hidden[HiddenNodes];
double Output[OutputNodes];
float HiddenWeights[3][4] = {{-0.5617644549564086, 1.0942893946497538, -1.0682572021248855, 1.304429946291011}, {0.8068076878837755, -4.010636506524299, 4.023499996501605, -0.3342896095417134}, {0.2122339991590681, -1.5699936019785983, -1.6506637473608512, 0.22538199584563273}};
float OutputWeights[4][4] = {{-1.371854399267592, -0.8997542122405253, -0.8681163843245009, 0.025905325213148457}, {-0.9808612938288105, -0.33757346882199263, -1.7677793863252733, 1.8239271557047205}, {-1.4958911354789663, 1.6436091911381543, 0.23187132309438943, 0.6119086932832044}, {1.0923727805142454, 1.2928498659973917, 1.3430060270009385, 1.5636986805649136}};
/******************************************************************
   End Network Configuration
 ******************************************************************/
 
void stop() {
  digitalWrite(ENA, LOW); //We deactivate the engines
  digitalWrite(ENB, LOW); //We deactivate the engines
  Serial.println("Stop!");
} 
 
//Measure distance in Centimeters
int Distance_test() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);
  float Fdistance = pulseIn(Echo, HIGH);
  Fdistance= Fdistance / 58;
  return (int)Fdistance;
}
 
void setup() {
  myservo.attach(3);  // attach servo on pin 3 to servo object
  Serial.begin(9600);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  stop();
  myservo.write(90);  //starting position in the center
  delay(500); 
} 
 
unsigned long previousMillis = 0;   // to measure time cycles
const long interval = 25;           // intervals every x milliseconds
int grados_servo = 90;                // position of the servo that moves the ultrasonic sensor
bool clockwise = true;              // direction of servo rotation
const long ANGULO_MIN = 30; 
const long ANGULO_MAX = 150; 
double ditanciaMaxima = 50.0;       // far distance from which the NN begins to act
int incrementos = 9;                // increments per cycle of servo position
int accionEnCurso = 1;              // number of cycles executing an action
int multiplicador = 1000/interval;  // multiply the number of cycles to give the car time to turn
const int SPEED = 100;              // speed of the car from all 4 wheels at a time
 
void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
 
  /******************************************************************
       HANDLE SERVO TURN
  ******************************************************************/
    if(grados_servo<=ANGULO_MIN || grados_servo>=ANGULO_MAX){
      clockwise=!clockwise; // cambio de sentido
      grados_servo = constrain(grados_servo, ANGULO_MIN, ANGULO_MAX);
    }
    if(clockwise)
      grados_servo=grados_servo+incrementos;
    else
      grados_servo=grados_servo-incrementos;
    
    if(accionEnCurso>0){
      accionEnCurso=accionEnCurso-1;
    }else{
  /******************************************************************
        WE CALL THE DRIVING FUNCTION
  ******************************************************************/
       conducir();      
    }
    myservo.write(grados_servo);    
  }
 
}
 
//USE THE NEURONAL NETWORK ALREADY TRAINED
void conducir()
{
    double TestInput[] = {0, 0,0};
    double entrada1=0,entrada2=0;
    
  /******************************************************************
    GET SENSOR DISTANCE
  ******************************************************************/
    double distance = double(Distance_test());
    distance= double(constrain(distance, 0.0, ditanciaMaxima));
    entrada1= ((-2.0/ditanciaMaxima)*double(distance))+1.0; //use a linear function to get closeness
    accionEnCurso = ((entrada1 +1) * multiplicador)+1; // If it is very close to the obstacle, it need more time to react
 
  /******************************************************************
    GET DIRECTION ACCORDING TO THE SERVO ANGLE
  ******************************************************************/
    entrada2 = map(grados_servo, ANGULO_MIN, ANGULO_MAX, -100, 100);
    entrada2 = double(constrain(entrada2, -100.00, 100.00));
 
  /******************************************************************
    WE CALL THE FEED FORWARD NETWORK WITH THE TICKETS
  ******************************************************************/
  Serial.print("Entrada1:");
  Serial.println(entrada1);
  Serial.print("Entrada2:");
  Serial.println(entrada2/100.0);
 
  TestInput[0] = 1.0;//BIAS UNIT
  TestInput[1] = entrada1;
  TestInput[2] = entrada2/100.0;
 
  InputToOutput(TestInput[0], TestInput[1], TestInput[2]); //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("Salida1:");
  Serial.println(out1);
  Serial.print("Salida2:");
  Serial.println(out2);
  Serial.println(Output[1]);
  Serial.print("Salida3:");
  Serial.println(out3);
  Serial.print("Salida4:");
  Serial.println(out4);
 
  /******************************************************************
    POWERING MOTORS WITH THE NETWORK OUTPUT
  ******************************************************************/
  int carSpeed = SPEED; //forward or backward
  if((out1+out3)==2 || (out2+out4)==2){ // if it is twist, the motors need double power
    carSpeed = SPEED * 2;
  }
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, out1 * HIGH); 
  digitalWrite(IN2, out2 * HIGH); 
  digitalWrite(IN3, out3 * HIGH);
  digitalWrite(IN4, out4 * HIGH);
}
 
void InputToOutput(double In1, double In2, double In3)
{
  double TestInput[] = {0, 0,0};
  TestInput[0] = In1;
  TestInput[1] = In2;
  TestInput[2] = In3;
 
  /******************************************************************
    Calculate triggers on hidden layers
  ******************************************************************/
 
  for ( i = 0 ; i < HiddenNodes ; i++ ) {
    Accum = 0;//HiddenWeights[InputNodes][i] ;
    for ( j = 0 ; j < InputNodes ; j++ ) {
      Accum += TestInput[j] * HiddenWeights[j][i] ;
    }
    //Hidden[i] = 1.0 / (1.0 + exp(-Accum)) ; //Sigmoid
    Hidden[i] = tanh(Accum) ; //tanh
  }
 
  /******************************************************************
    Calculate activation and error in the Output layer
  ******************************************************************/
 
  for ( i = 0 ; i < OutputNodes ; i++ ) {
    Accum = 0;//OutputWeights[HiddenNodes][i];
    for ( j = 0 ; j < HiddenNodes ; j++ ) {
        Accum += Hidden[j] * OutputWeights[j][i] ;
    }
    Output[i] = tanh(Accum) ;//tanh
  }
}

In it an L298N driver is used to control the motors, but I have an L293D. So...

It is there a way it could work with my driver instead.
What do I need to modify for this code to work with my driver?

Thanks for your help

They are functionally the same. Just make sure the pins are connected appropriately to the L293D.

By the way, where did these (extremely important) weights come from and what do they do?

float HiddenWeights[3][4] = {{-0.5617644549564086, 1.0942893946497538, -1.0682572021248855, 1.304429946291011}, {0.8068076878837755, -4.010636506524299, 4.023499996501605, -0.3342896095417134}, {0.2122339991590681, -1.5699936019785983, -1.6506637473608512, 0.22538199584563273}};
float OutputWeights[4][4] = {{-1.371854399267592, -0.8997542122405253, -0.8681163843245009, 0.025905325213148457}, {-0.9808612938288105, -0.33757346882199263, -1.7677793863252733, 1.8239271557047205}, {-1.4958911354789663, 1.6436091911381543, 0.23187132309438943, 0.6119086932832044}, {1.0923727805142454, 1.2928498659973917, 1.3430060270009385, 1.5636986805649136}};

How was this network trained?

They came from a python script, where they were calculated, it can be found here: machine-learning/Red_Neuronal_coche.ipynb at master · jbagnato/machine-learning · GitHub. Supposedly, they output 0 to 1 indicating whether or not to turn on the engines of an Arduino car.

I managed to do the next changes to the original script to make the robot "work" with the L293D driver, but I cant get to react to the wieghts it just go forward and when an obstable is present it backwards then go forward again to the same obstacle...

#include <Servo.h>  //servo library
Servo myservo;      // create servo object to control servo
 #include <AFMotor.h>
#define Echo  A1 
#define Trig  A0
#define IN11 1
#define IN22 2
#define IN33 3
#define IN44 4

AF_DCMotor IN1(IN11, MOTOR12_1KHZ); 
AF_DCMotor IN2(IN22, MOTOR12_1KHZ);
AF_DCMotor IN3(IN33, MOTOR34_1KHZ);
AF_DCMotor IN4(IN44, MOTOR34_1KHZ);
/******************************************************************
   Network Configuration
 ******************************************************************/
const int InputNodes = 3; // includes BIAS neuron
const int HiddenNodes = 4; //includes BIAS neuron
const int OutputNodes = 4;
int i, j;
double Accum;
double Hidden[HiddenNodes];
double Output[OutputNodes];
float HiddenWeights[3][4] = {{-0.5617644549564086, 1.0942893946497538, -1.0682572021248855, 1.304429946291011}, {0.8068076878837755, -4.010636506524299, 4.023499996501605, -0.3342896095417134}, {0.2122339991590681, -1.5699936019785983, -1.6506637473608512, 0.22538199584563273}};
float OutputWeights[4][4] = {{-1.371854399267592, -0.8997542122405253, -0.8681163843245009, 0.025905325213148457}, {-0.9808612938288105, -0.33757346882199263, -1.7677793863252733, 1.8239271557047205}, {-1.4958911354789663, 1.6436091911381543, 0.23187132309438943, 0.6119086932832044}, {1.0923727805142454, 1.2928498659973917, 1.3430060270009385, 1.5636986805649136}};
/******************************************************************
   End Network Configuration
 ******************************************************************/
 
void stop() {
  IN1.run(BACKWARD);      
  IN2.run(BACKWARD);
  IN3.run(BACKWARD); 
  IN4.run(BACKWARD);
  Serial.println("Stop!");
} 

//Measure distance in Centimeters
int Distance_test() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);
  float Fdistance = pulseIn(Echo, HIGH);
  Fdistance= Fdistance / 58;
  Serial.print("CM: ");
  Serial.println(Fdistance);
  return (int)Fdistance;
}

void setup() {
  myservo.attach(10);  // attach servo on pin 3 to servo object
  Serial.begin(9600);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  stop();
  myservo.write(90);  //starting position in the center
  delay(500); 
} 
 
unsigned long previousMillis = 0;   // to measure time cycles
const long interval = 25;           // intervals every x milliseconds
int grados_servo = 90;              // position of the servo that moves the ultrasonic sensor
bool clockwise = true;              // direction of servo rotation
const long ANGULO_MIN = 30; 
const long ANGULO_MAX = 150; 
double ditanciaMaxima = 50.0;       // far distance from which the NN begins to act
int incrementos = 9;                // increments per cycle of servo position
int accionEnCurso = 1;              // number of cycles executing an action
int multiplicador = 1000/interval;  // multiply the number of cycles to give the car time to turn
const int SPEED = 100;              // speed of the car from all 4 wheels at a time
 
void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
 
  /******************************************************************
       HANDLE SERVO TURN
  ******************************************************************/
    if(grados_servo<=ANGULO_MIN || grados_servo>=ANGULO_MAX){
      clockwise=!clockwise; // cambio de sentido
      grados_servo = constrain(grados_servo, ANGULO_MIN, ANGULO_MAX);
    }
    if(clockwise)
      grados_servo=grados_servo+incrementos;
    else
      grados_servo=grados_servo-incrementos;
    
    if(accionEnCurso>0){
      accionEnCurso=accionEnCurso-1;
    }else{
  /******************************************************************
        WE CALL THE DRIVING FUNCTION
  ******************************************************************/
    conducir();      
    }
    myservo.write(grados_servo);    
  }
}
 
//USE THE NEURONAL NETWORK ALREADY TRAINED
void conducir()
{
  double TestInput[] = {0, 0,0};
  double entrada1=0,entrada2=0;
    
  /******************************************************************
    GET SENSOR DISTANCE
  ******************************************************************/
  double distance = double(Distance_test());
  distance= double(constrain(distance, 0.0, ditanciaMaxima));
  entrada1= ((-2.0/ditanciaMaxima)*double(distance))+1.0; //use a linear function to get closeness
  accionEnCurso = ((entrada1 +1) * multiplicador)+1; // If it is very close to the obstacle, it need more time to react
 
  /******************************************************************
    GET DIRECTION ACCORDING TO THE SERVO ANGLE
  ******************************************************************/
  entrada2 = map(grados_servo, ANGULO_MIN, ANGULO_MAX, -100, 100);
  entrada2 = double(constrain(entrada2, -100.00, 100.00));
 
  /******************************************************************
    WE CALL THE FEED FORWARD NETWORK WITH THE TICKETS
  ******************************************************************/
  // Serial.print("Entrada1:");
  // Serial.println(entrada1);
  // Serial.print("Entrada2:");
  // Serial.println(entrada2/100.0);
 
  TestInput[0] = 1.0;//BIAS UNIT
  TestInput[1] = entrada1;
  TestInput[2] = entrada2/100.0;
 
  InputToOutput(TestInput[0], TestInput[1], TestInput[2]); //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("Salida1:");
  //Serial.println(out1);
  //Serial.print("Salida2:");
  //Serial.println(out2);
  //Serial.println(Output[1]);
  //Serial.print("Salida3:");
  //Serial.println(out3);
  //Serial.print("Salida4:");
  //Serial.println(out4);
 
  /******************************************************************
    POWERING MOTORS WITH THE NETWORK OUTPUT
  ******************************************************************/
  int carSpeed = SPEED; //forward or backward
  if((out1+out3)==2 || (out2+out4)==2){ // if it is twist, the motors need double power
    carSpeed = SPEED * 2;
  }
  IN1.setSpeed(carSpeed);
  IN2.setSpeed(carSpeed);
  IN3.setSpeed(carSpeed);
  IN4.setSpeed(carSpeed);
  if (out1==1 && out2==0 && out3 ==0 && out4==1)
  {
    IN1.run(FORWARD);      
    IN2.run(FORWARD);
    IN3.run(FORWARD); 
    IN4.run(FORWARD);
    Serial.print("Forward Direction");
    Serial.println("");
  }
  else if (out1==0 && out2==1 && out3 ==1 && out4==0)
  {
    IN1.run(BACKWARD);      
    IN2.run(BACKWARD);
    IN3.run(BACKWARD); 
    IN4.run(BACKWARD);
    Serial.print("Backward Direction");
    Serial.println("");
  }
  else if(out1==1 && out2==0 && out3 ==1 && out4==0)
  {
    IN1.run(BACKWARD);      
    IN2.run(BACKWARD);
    IN3.run(FORWARD); 
    IN4.run(FORWARD);
    Serial.print("LEFT direction");
    Serial.println("");
  }
  else if (out1==0 && out2==1 && out3 ==0 && out4==1)
  {
    IN1.run(FORWARD);      
    IN2.run(FORWARD);
    IN3.run(BACKWARD); 
    IN4.run(BACKWARD);
    Serial.print("Right direction");
    Serial.println("");
  }
}

void InputToOutput(double In1, double In2, double In3)
{
  double TestInput[] = {0, 0,0};
  TestInput[0] = In1;
  TestInput[1] = In2;
  TestInput[2] = In3;
 
  /******************************************************************
    Calculate triggers on hidden layers
  ******************************************************************/
 
  for ( i = 0 ; i < HiddenNodes ; i++ ) {
    Accum = 0;//HiddenWeights[InputNodes][i] ;
    for ( j = 0 ; j < InputNodes ; j++ ) {
      Accum += TestInput[j] * HiddenWeights[j][i] ;
    }
    //Hidden[i] = 1.0 / (1.0 + exp(-Accum)) ; //Sigmoid
    Hidden[i] = tanh(Accum) ; //tanh
  }
 
  /******************************************************************
    Calculate activation and error in the Output layer
  ******************************************************************/
 
  for ( i = 0 ; i < OutputNodes ; i++ ) {
    Accum = 0;//OutputWeights[HiddenNodes][i];
    for ( j = 0 ; j < HiddenNodes ; j++ ) {
        Accum += Hidden[j] * OutputWeights[j][i] ;
    }
    Output[i] = tanh(Accum) ;//tanh
  }
}

To debug the neural network output, you need to activate (uncomment) print statements like these, and see whether the outputs make sense.

  //Serial.print("Salida1:");
  //Serial.println(out1);
  //Serial.print("Salida2:");
  //Serial.println(out2);
  //Serial.println(Output[1]);
  //Serial.print("Salida3:");
  //Serial.println(out3);
  //Serial.print("Salida4:");
  //Serial.println(out4);

Thanks for your help, I didi uncomment those lines, and this is the output, its always the same when sensor is blocked an unblocked

WhatsApp Image 2021-12-12 at 14.05.35

What do those outputs mean for your robot?

According to the second table in the image first output is to go backwards and next is to go forward

WhatsApp Image 2021-12-11 at 22.55.14

That appears to explain the behavior you see. The neural network doesn't seem to be doing you much good.

With only one distance input, how should the robot decide between turn right, turn left and back up?

In the original script with the L298N driver, each number gives an order to just one motor...

image

Then, depending on the output combination, the motors react accordingly.

I tried an approach really close to this, but didn´t work at all,

  IN1.run(out1*FORWARD);   
  IN2.run(out2*FORWARD);
  IN3.run(out3*FORWARD); // Devolver a FORWARD
  IN4.run(out4*FORWARD); // Devolver a FORWARD

Tried many other combos of conditional blocks, but apparently its because the output of the NN its rounded, but, for example if the NN trows a 0.49 it is rounded to 0, then that motor doesn't behave as if an obstacle is present. Then if the NN trow 0.49, 0.00, 0.99, 0.00 it is equal to 0, 0, 1 , 0, that is not present in the training table.

Or thats what I think is happening...