4
This commit is contained in:
reto
866
src/main.cpp
Normal file
866
src/main.cpp
Normal file
@ -0,0 +1,866 @@
|
||||
#include <Arduino.h>
|
||||
/******************************************************************************
|
||||
* Dinoi Follower Neuronal Network Version Arduino Mini Pro 5V
|
||||
*
|
||||
* Follower Neuronal Network
|
||||
*
|
||||
* Arduino NN - An artificial neural network for the Arduino
|
||||
*
|
||||
* Based on Neuronal Network from robotics.hobbizine.com/arduinoann.html
|
||||
******************************************************************************/
|
||||
|
||||
#include <math.h>
|
||||
|
||||
//*******************************************
|
||||
// Smart Debug
|
||||
//*******************************************
|
||||
// Debugging einschalten, zum ausschalten auskommentieren
|
||||
#define _SMARTDEBUG
|
||||
|
||||
char f[10];
|
||||
|
||||
// Debug Makros
|
||||
#ifdef _SMARTDEBUG
|
||||
|
||||
// Hilfsfunktion für WAIT - Makro
|
||||
void DebugWait(String txt)
|
||||
{
|
||||
// buffer leeren
|
||||
char ch;
|
||||
while (Serial.available())
|
||||
ch = Serial.read();
|
||||
ch = 0;
|
||||
|
||||
Serial.print(txt);
|
||||
Serial.println(" >press 'c' to continue...");
|
||||
|
||||
// auf 'c' warten
|
||||
do
|
||||
{
|
||||
if (Serial.available() > 0)
|
||||
ch = Serial.read();
|
||||
} while (ch != 'c');
|
||||
// buffer leeren
|
||||
while (Serial.available())
|
||||
ch = Serial.read();
|
||||
}
|
||||
|
||||
#define DEBUG_INIT(speed) Serial.begin(speed)
|
||||
#define DEBUG_PRINTLN(txt) Serial.println(txt)
|
||||
#define DEBUG_PRINT(txt) Serial.print(txt)
|
||||
#define DEBUG_PRINTLN_VALUE(txt, val) \
|
||||
Serial.print(txt); \
|
||||
Serial.print(": "); \
|
||||
dtostrf(val, 6, 3, f); \
|
||||
Serial.println(f)
|
||||
#define DEBUG_PRINT_VALUE(txt, val) \
|
||||
Serial.print(txt); \
|
||||
Serial.print(": "); \
|
||||
dtostrf(val, 6, 3, f); \
|
||||
Serial.print(f)
|
||||
#define DEBUG_WAIT(txt, condition) \
|
||||
if (condition) \
|
||||
DebugWait(txt)
|
||||
#else
|
||||
#define DEBUG_INIT(speed)
|
||||
#define DEBUG_PRINT(txt)
|
||||
#define DEBUG_PRINTLN(txt)
|
||||
#define DEBUG_PRINT_VALUE(txt, val)
|
||||
#define DEBUG_PRINTLN_VALUE(txt, val)
|
||||
#define DEBUG_WAIT(txt, condition)
|
||||
#endif
|
||||
|
||||
/******************************************************************************
|
||||
/* Pin Configuration
|
||||
/******************************************************************************/
|
||||
#define MDL 4 // Motor Direction Left
|
||||
#define MSL 5 // Motor Speed Left
|
||||
#define MSR 6 // Motor Speed Right
|
||||
#define MDR 7 // Motor Direction Right
|
||||
|
||||
int IrLedVr = 3;
|
||||
int IrLedVl = 2;
|
||||
int IrLedHr = 10;
|
||||
int IrLedHl = 9;
|
||||
|
||||
int IrRecVr = A3;
|
||||
int IrRecVl = A2;
|
||||
int IrRecHr = A0;
|
||||
int IrRecHl = A1;
|
||||
const int ledPin = 13; // the number of the LED pin
|
||||
|
||||
/* MOTOR DIFFERENCE CONFIG
|
||||
/******************************************************************************/
|
||||
#define DIST 65 // Define Minimum Distance
|
||||
|
||||
// dino
|
||||
/*
|
||||
#define MTRF 220 // Motor Topspeed Right Forward
|
||||
#define MTRB 220 // Motor Topspeed Right Backward
|
||||
#define MTLF 187 // Motor Topspeed Left Forward
|
||||
#define MTLB 189 // Motor Topspeed Left Backward
|
||||
*/
|
||||
|
||||
#define MTRF 230 // Motor Topspeed Right Forward
|
||||
#define MTLB 220 // Motor Topspeed Left Backward / Forw
|
||||
#define MTRB 200 // Motor Topspeed Right Backward
|
||||
#define MTLF 220 // Motor Topspeed Left Forward / Backw
|
||||
|
||||
#define minIR 20
|
||||
#define maxIR 220
|
||||
|
||||
// normal
|
||||
/*
|
||||
#define MTRF 200 // Motor Topspeed Right Forward
|
||||
#define MTLB 170 // Motor Topspeed Left Backward / Forw
|
||||
#define MTRB 200 // Motor Topspeed Right Backward
|
||||
#define MTLF 170 // Motor Topspeed Left Forward / Backw
|
||||
#define minIR 0
|
||||
#define maxIR 220
|
||||
*/
|
||||
|
||||
// Variables
|
||||
int lspeed = 0; // current speed of left motor
|
||||
int rspeed = 0; // current speed of right motor
|
||||
int spause = 1; // wait time if a whisker was touched
|
||||
bool protsta = false;
|
||||
bool motsta = true;
|
||||
|
||||
/******************************************************************
|
||||
* Network Configuration - customized per network
|
||||
******************************************************************/
|
||||
// Logic
|
||||
// Sensor -255 to 255
|
||||
// Motor -255 to 255
|
||||
// Normal forward => Sensor 150 => Motor 150
|
||||
// Obstacle => Sensor 80 => 80
|
||||
|
||||
const int PatternCount = 6; // The number of training items or rows in the truth table
|
||||
const int InputNodes = 2; // The number of input neurons
|
||||
const int OutputNodes = 2; // The number of output neurons
|
||||
|
||||
const int HiddenNodes = 5; // The number of hidden neurons
|
||||
const float LearningRate = 0.3; // The number of Learning Rate
|
||||
const float Momentum = 0.9; // Adjusts how much the results of the previous iteration affect the current iteration
|
||||
const float InitialWeightMax = 0.5; // Sets the maximum starting value for weights.
|
||||
float Success = 0.101; // 0.02 0.01 // Level of minimum Success
|
||||
|
||||
// Obsacle Mode with return
|
||||
//Success = 0.04; // Level of minimum Success
|
||||
/*
|
||||
float Input[PatternCount][InputNodes] = {
|
||||
{1, 1}, // No Obstactle
|
||||
{0.8, 0.8}, // No Obstactle
|
||||
{0.4, 1}, // Obstacle on left
|
||||
{1, 0.4}, // Obstacle on right
|
||||
{0.3, 0.3}, // Obstacle left and right
|
||||
{0, 0}, // Obstacle left and right
|
||||
};
|
||||
|
||||
const float Target[PatternCount][OutputNodes] = {
|
||||
{1, 1}, // No Obstactle
|
||||
{0.8, 0.8}, // No Obstactle
|
||||
{0.6, -0.3}, // Obstacle on left
|
||||
{-0.3, 0.6}, // Obstacle on right
|
||||
{0.1, 0.1}, // Obstacle left and right
|
||||
{0, 0}, // Obstacle left and right
|
||||
};
|
||||
*/
|
||||
|
||||
// Obsacle Mode
|
||||
//Success = 0.04; // Level of minimum Success
|
||||
|
||||
float Input[PatternCount][InputNodes] = {
|
||||
{ 1, 1 }, // No Obstactle
|
||||
{ 0.8, 0.8 }, // No Obstactle
|
||||
{ 0.8, 1 }, // Obstacle on left
|
||||
{ 1, 0.8 }, // Obstacle on right
|
||||
{ 0.3, 0.3 }, // Obstacle left and right
|
||||
{ 0, 0 }, // Obstacle left and right
|
||||
};
|
||||
|
||||
const float Target[PatternCount][OutputNodes] = {
|
||||
{ 1, 1 }, // No Obstactle
|
||||
{ 0.8, 0.8 }, // No Obstactle
|
||||
{ 0.8, 0.3 }, // Obstacle on left
|
||||
{ 0.3, 0.8 }, // Obstacle on right
|
||||
{ 0.1, 0.1 }, // Obstacle left and right
|
||||
{ 0, 0 }, // Obstacle left and right
|
||||
};
|
||||
|
||||
// OK NN
|
||||
/*
|
||||
float Input[PatternCount][InputNodes] = {
|
||||
{1, 1}, // No Obstactle
|
||||
{0.8, 0.8}, // No Obstactle
|
||||
{0.5, 1}, // Obstacle on left
|
||||
{1, 0.5}, // Obstacle on right
|
||||
{0.3, 0.3}, // Obstacle left and right
|
||||
{0, 0}, // Obstacle left and right
|
||||
};
|
||||
|
||||
const float Target[PatternCount][OutputNodes] = {
|
||||
{1, 1}, // No Obstactle
|
||||
{0.7, 0.7}, // No Obstactle
|
||||
{0.4, 1}, // Obstacle on left
|
||||
{1, 0.4}, // Obstacle on right
|
||||
{0.1, 0.1}, // Obstacle left and right
|
||||
{0, 0}, // Obstacle left and right
|
||||
};
|
||||
*/
|
||||
|
||||
// Good NN Agressive
|
||||
/*
|
||||
float Input[PatternCount][InputNodes] = {
|
||||
{ 1, 1 }, // No Obstactle
|
||||
{ 0.8, 0.8 },// No Obstactle
|
||||
{ 0.3, 1 }, // Obstacle on left
|
||||
{ 1, 0.3 }, // Obstacle on right
|
||||
{ 0.3, 0.3 }, // Obstacle left and right
|
||||
};
|
||||
|
||||
const float Target[PatternCount][OutputNodes] = {
|
||||
{ 1, 1 }, // No Obstactle
|
||||
{ 0.7, 0.7 }, // No Obstactle
|
||||
{ 0, 1 }, // Obstacle on left
|
||||
{ 1, 0 }, // Obstacle on right
|
||||
{ 0, 0 }, // Obstacle left and right
|
||||
};
|
||||
*/
|
||||
|
||||
// last ok
|
||||
/*
|
||||
float Input[PatternCount][InputNodes] = {
|
||||
{ 1, 1 }, // No Obstactle
|
||||
{ 0.65, 0.65 },// No Obstactle
|
||||
{ 0.22, 1 }, // Obstacle on left
|
||||
{ 1, 0.22 }, // Obstacle on right
|
||||
{ 0.22, 0.22 }, // Obstacle left and right
|
||||
{ 0.11, 0.11 }, // Obstacle left and right close
|
||||
};
|
||||
|
||||
const float Target[PatternCount][OutputNodes] = {
|
||||
{ 1, 1 }, // No Obstactle
|
||||
{ 0.9, 0.9 }, // No Obstactle
|
||||
{ 1, 0.2 }, // Obstacle on left
|
||||
{ 0.2, 1 }, // Obstacle on right
|
||||
{ 0.7, 0.7 }, // Obstacle left and right
|
||||
{ 0.5, 0.5 }, // Obstacle left and right close
|
||||
};
|
||||
*/
|
||||
|
||||
/******************************************************************
|
||||
* Network Variables
|
||||
******************************************************************/
|
||||
char mode; //(t => train,c=>check, r => run, a = analyze)
|
||||
int i, j, p, q, r;
|
||||
int ReportEvery1000;
|
||||
int RandomizedIndex[PatternCount];
|
||||
long TrainingCycle;
|
||||
float Rando;
|
||||
float Error;
|
||||
float Accum;
|
||||
|
||||
float Hidden[HiddenNodes];
|
||||
float Output[OutputNodes];
|
||||
float HiddenWeights[InputNodes + 1][HiddenNodes];
|
||||
float OutputWeights[HiddenNodes + 1][OutputNodes];
|
||||
float HiddenDelta[HiddenNodes];
|
||||
float OutputDelta[OutputNodes];
|
||||
float ChangeHiddenWeights[InputNodes + 1][HiddenNodes];
|
||||
float ChangeOutputWeights[HiddenNodes + 1][OutputNodes];
|
||||
|
||||
/******************************************************************************
|
||||
/* Setup
|
||||
/******************************************************************************/
|
||||
void setup()
|
||||
{
|
||||
|
||||
// Neuronal Setup
|
||||
/******************************************************************************/
|
||||
randomSeed(analogRead(3));
|
||||
ReportEvery1000 = 1;
|
||||
for (p = 0; p < PatternCount; p++)
|
||||
{
|
||||
RandomizedIndex[p] = p;
|
||||
}
|
||||
delay(200);
|
||||
mode = 't'; // ( t = train, a = analyze sensors )
|
||||
|
||||
// Robo motors Setup
|
||||
/******************************************************************************/
|
||||
pinMode(MDL, OUTPUT);
|
||||
pinMode(MSL, OUTPUT);
|
||||
pinMode(MSR, OUTPUT);
|
||||
pinMode(MDR, OUTPUT);
|
||||
|
||||
// all IR Leds / Recevier
|
||||
pinMode(ledPin, OUTPUT);
|
||||
pinMode(IrLedVr, OUTPUT);
|
||||
pinMode(IrLedVl, OUTPUT);
|
||||
pinMode(IrLedHr, OUTPUT);
|
||||
pinMode(IrLedHl, OUTPUT);
|
||||
pinMode(IrRecVr, INPUT);
|
||||
pinMode(IrRecVl, INPUT);
|
||||
pinMode(IrRecHr, INPUT);
|
||||
pinMode(IrRecHl, INPUT);
|
||||
|
||||
// Serial Debug Interface
|
||||
DEBUG_INIT(57600);
|
||||
DEBUG_PRINTLN("Robot Starting with training");
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* InputToOutput
|
||||
/******************************************************************************/
|
||||
void InputToOutput(float In1, float In2)
|
||||
{
|
||||
float TestInput[] = {0, 0};
|
||||
TestInput[0] = In1;
|
||||
TestInput[1] = In2;
|
||||
|
||||
/******************************************************************
|
||||
Compute hidden layer activations
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < HiddenNodes; i++)
|
||||
{
|
||||
Accum = HiddenWeights[InputNodes][i];
|
||||
for (j = 0; j < InputNodes; j++)
|
||||
{
|
||||
Accum += TestInput[j] * HiddenWeights[j][i];
|
||||
}
|
||||
Hidden[i] = 1.0 / (1.0 + exp(-Accum));
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
Compute output layer activations and calculate errors
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
Accum = OutputWeights[HiddenNodes][i];
|
||||
for (j = 0; j < HiddenNodes; j++)
|
||||
{
|
||||
Accum += Hidden[j] * OutputWeights[j][i];
|
||||
}
|
||||
Output[i] = 1.0 / (1.0 + exp(-Accum));
|
||||
}
|
||||
/* DEBUG_PRINT(" Output Nodes");
|
||||
for ( i = 0 ; i < OutputNodes ; i++ ) {
|
||||
DEBUG_PRINT_VALUE(" ",Output[i]);
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
* Funktion: irRead (a,b)
|
||||
* a: Pinnummer des Ir Empfängers
|
||||
* b: Pinnummer des Ir Senders
|
||||
* c: modedeb: Space = Normalmodus returns HIGH if > DIST
|
||||
* d = Distancemodus return Distance 0 to 100 mm
|
||||
* Retourwert: HIGH = IR-Licht detektiert
|
||||
* d.h. gemessener Wert unterscheidet sich zum Umgebungslicht
|
||||
******************************************************************************/
|
||||
int irRead(int readPin, int triggerPin, char modedeb)
|
||||
{
|
||||
boolean zustand;
|
||||
int umgebungswert = 0;
|
||||
int irwert = 0;
|
||||
float uLichtzuIr;
|
||||
|
||||
for (int i = 0; i < 10; i++)
|
||||
{
|
||||
digitalWrite(triggerPin, LOW);
|
||||
delay(5);
|
||||
umgebungswert = umgebungswert + analogRead(readPin);
|
||||
digitalWrite(triggerPin, HIGH);
|
||||
delay(5);
|
||||
irwert = irwert + analogRead(readPin);
|
||||
}
|
||||
|
||||
// Remove Umgebungswert
|
||||
irwert = irwert - umgebungswert;
|
||||
|
||||
// detect obstacle
|
||||
if (irwert > DIST)
|
||||
{
|
||||
zustand = HIGH;
|
||||
}
|
||||
else
|
||||
{
|
||||
zustand = LOW;
|
||||
}
|
||||
|
||||
// for Debug
|
||||
if (protsta && zustand || modedeb == 'd')
|
||||
{
|
||||
delay(5);
|
||||
if (modedeb == 'd')
|
||||
{
|
||||
//Serial.print(" irwert ");
|
||||
//Serial.print(irwert);
|
||||
irwert = map(irwert, minIR, maxIR, 100, 0);
|
||||
constrain(irwert, 0, 100);
|
||||
//Serial.print(" dis ");
|
||||
//Serial.print(irwert);
|
||||
};
|
||||
return irwert;
|
||||
}
|
||||
else
|
||||
{
|
||||
return zustand;
|
||||
};
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Funktion: Drive
|
||||
/******************************************************************************/
|
||||
void drive(int left, int right)
|
||||
{
|
||||
int lstate = LOW;
|
||||
int rstate = LOW;
|
||||
|
||||
left = constrain(left, -255, 255);
|
||||
right = constrain(right, -255, 255);
|
||||
|
||||
// invert left
|
||||
left = left * -1;
|
||||
// invert right
|
||||
//right = right * -1;
|
||||
|
||||
if (left >= 0)
|
||||
{
|
||||
// remap the motor value to the calibrated interval
|
||||
// speed is alwas handled in the range of [-255..255]
|
||||
left = map(left, 0, 255, 0, MTLF);
|
||||
}
|
||||
else
|
||||
{
|
||||
lstate = HIGH;
|
||||
left = 255 - map(-left, 0, 255, 0, MTLB);
|
||||
}
|
||||
if (right >= 0)
|
||||
{
|
||||
right = map(right, 0, 255, 0, MTRF);
|
||||
}
|
||||
else
|
||||
{
|
||||
rstate = HIGH;
|
||||
right = 255 - map(-right, 0, 255, 0, MTRB);
|
||||
}
|
||||
analogWrite(MSL, left);
|
||||
digitalWrite(MDL, lstate);
|
||||
|
||||
analogWrite(MSR, right);
|
||||
digitalWrite(MDR, rstate);
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Run NN
|
||||
/******************************************************************************/
|
||||
void run_nn()
|
||||
{
|
||||
|
||||
delay(30);
|
||||
|
||||
// Get IR Values
|
||||
rspeed = irRead(IrRecVr, IrLedVr, 'd');
|
||||
lspeed = irRead(IrRecVl, IrLedVl, 'd');
|
||||
|
||||
//DEBUG_PRINT_VALUE("R IR",rspeed);
|
||||
//DEBUG_PRINT_VALUE(" L IR",lspeed);
|
||||
|
||||
// Convert IR to float from 0 to 1
|
||||
float f_rspeed = rspeed;
|
||||
float f_lspeed = lspeed;
|
||||
;
|
||||
f_lspeed = f_lspeed / 100;
|
||||
f_rspeed = f_rspeed / 100;
|
||||
|
||||
DEBUG_PRINT_VALUE(" R IR", f_rspeed * 100);
|
||||
DEBUG_PRINT_VALUE(" L IR", f_lspeed * 100);
|
||||
|
||||
// Ask solution from NN
|
||||
InputToOutput(f_lspeed, f_rspeed); //INPUT to ANN to obtain OUTPUT
|
||||
|
||||
DEBUG_PRINT_VALUE(" NN Out R Mot", Output[1] * 100);
|
||||
DEBUG_PRINTLN_VALUE(" L Mot", Output[0] * 100);
|
||||
|
||||
rspeed = Output[1] * 200;
|
||||
lspeed = Output[0] * 200;
|
||||
|
||||
//DEBUG_PRINT_VALUE(" - Set Speed R MOT",rspeed);
|
||||
//DEBUG_PRINTLN_VALUE(" L MOT",lspeed);
|
||||
|
||||
// Drive Motors
|
||||
drive(lspeed, rspeed);
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Run NN
|
||||
/******************************************************************************/
|
||||
void analyze_sensors()
|
||||
{
|
||||
|
||||
delay(30);
|
||||
|
||||
// Get IR Values
|
||||
rspeed = irRead(IrRecVr, IrLedVr, 'd');
|
||||
lspeed = irRead(IrRecVl, IrLedVl, 'd');
|
||||
|
||||
DEBUG_PRINT_VALUE("R IR", rspeed);
|
||||
DEBUG_PRINT_VALUE(" L IR", lspeed);
|
||||
|
||||
// IR from -100 to +200
|
||||
// rspeed = constrain(rspeed,-200,200);
|
||||
// lspeed = constrain(lspeed,-200,200);
|
||||
|
||||
// Convert IR to float from 0 to 1
|
||||
float f_rspeed = rspeed;
|
||||
float f_lspeed = lspeed;
|
||||
;
|
||||
f_lspeed = (200 + f_lspeed) / 400;
|
||||
f_rspeed = (200 + f_rspeed) / 400;
|
||||
|
||||
DEBUG_PRINT_VALUE(" - IN R FL IR", f_rspeed);
|
||||
DEBUG_PRINTLN_VALUE(" L FL IR", f_lspeed);
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Check NN
|
||||
/******************************************************************************/
|
||||
void check_nn()
|
||||
{
|
||||
|
||||
float TestInput[] = {0.9, 0.9};
|
||||
|
||||
DEBUG_PRINTLN("");
|
||||
DEBUG_PRINTLN("Test Run:");
|
||||
|
||||
// Test 1
|
||||
//********
|
||||
DEBUG_PRINT_VALUE("Test Input: L IR", TestInput[0]);
|
||||
DEBUG_PRINT_VALUE(" R IR", TestInput[1]);
|
||||
|
||||
InputToOutput(TestInput[0], TestInput[1]); //INPUT to ANN to obtain OUTPUT
|
||||
|
||||
DEBUG_PRINT_VALUE(" Out L Mot", Output[0]);
|
||||
DEBUG_PRINTLN_VALUE(" R Mot", Output[1]);
|
||||
|
||||
// Test 2
|
||||
//********
|
||||
TestInput[0] = 0.4;
|
||||
TestInput[1] = 0.9;
|
||||
|
||||
DEBUG_PRINT_VALUE("Test Input: L IR", TestInput[0]);
|
||||
DEBUG_PRINT_VALUE(" R IR", TestInput[1]);
|
||||
|
||||
InputToOutput(TestInput[0], TestInput[1]); //INPUT to ANN to obtain OUTPUT
|
||||
|
||||
DEBUG_PRINT_VALUE(" Out L Mot", Output[0]);
|
||||
DEBUG_PRINTLN_VALUE(" R Mot", Output[1]);
|
||||
|
||||
// Test 2
|
||||
//********
|
||||
TestInput[0] = 0.7;
|
||||
TestInput[1] = 0.3;
|
||||
|
||||
DEBUG_PRINT_VALUE("Test Input: L IR", TestInput[0]);
|
||||
DEBUG_PRINT_VALUE(" R IR", TestInput[1]);
|
||||
|
||||
InputToOutput(TestInput[0], TestInput[1]); //INPUT to ANN to obtain OUTPUT
|
||||
|
||||
DEBUG_PRINT_VALUE(" Out L Mot", Output[0]);
|
||||
DEBUG_PRINTLN_VALUE(" R Mot", Output[1]);
|
||||
|
||||
// go to run
|
||||
mode = 'r';
|
||||
DEBUG_PRINTLN(" ");
|
||||
DEBUG_PRINTLN("Go to run bot:");
|
||||
}
|
||||
|
||||
void toTerminal()
|
||||
{
|
||||
|
||||
for (p = 0; p < PatternCount; p++)
|
||||
{
|
||||
DEBUG_PRINTLN(" ");
|
||||
DEBUG_PRINT_VALUE(" Training Pattern", p);
|
||||
DEBUG_PRINT(" Input: ");
|
||||
for (i = 0; i < InputNodes; i++)
|
||||
{
|
||||
DEBUG_PRINT(Input[p][i]);
|
||||
DEBUG_PRINT(" ");
|
||||
}
|
||||
DEBUG_PRINT(" Target: ");
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
DEBUG_PRINT(Target[p][i]);
|
||||
DEBUG_PRINT(" ");
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Compute hidden layer activations
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < HiddenNodes; i++)
|
||||
{
|
||||
Accum = HiddenWeights[InputNodes][i];
|
||||
for (j = 0; j < InputNodes; j++)
|
||||
{
|
||||
Accum += Input[p][j] * HiddenWeights[j][i];
|
||||
}
|
||||
Hidden[i] = 1.0 / (1.0 + exp(-Accum));
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Compute output layer activations and calculate errors
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
Accum = OutputWeights[HiddenNodes][i];
|
||||
for (j = 0; j < HiddenNodes; j++)
|
||||
{
|
||||
Accum += Hidden[j] * OutputWeights[j][i];
|
||||
}
|
||||
Output[i] = 1.0 / (1.0 + exp(-Accum));
|
||||
}
|
||||
DEBUG_PRINT(" Output ");
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
DEBUG_PRINT(Output[i]);
|
||||
DEBUG_PRINT(" ");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Train NN
|
||||
/******************************************************************************/
|
||||
void train_nn()
|
||||
{
|
||||
/******************************************************************
|
||||
* Initialize HiddenWeights and ChangeHiddenWeights
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < HiddenNodes; i++)
|
||||
{
|
||||
for (j = 0; j <= InputNodes; j++)
|
||||
{
|
||||
ChangeHiddenWeights[j][i] = 0.0;
|
||||
Rando = float(random(100)) / 100;
|
||||
HiddenWeights[j][i] = 2.0 * (Rando - 0.5) * InitialWeightMax;
|
||||
}
|
||||
}
|
||||
/******************************************************************
|
||||
* Initialize OutputWeights and ChangeOutputWeights
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
for (j = 0; j <= HiddenNodes; j++)
|
||||
{
|
||||
ChangeOutputWeights[j][i] = 0.0;
|
||||
Rando = float(random(100)) / 100;
|
||||
OutputWeights[j][i] = 2.0 * (Rando - 0.5) * InitialWeightMax;
|
||||
}
|
||||
}
|
||||
DEBUG_PRINTLN("Initial/Untrained Outputs: ");
|
||||
toTerminal();
|
||||
/******************************************************************
|
||||
* Begin training
|
||||
******************************************************************/
|
||||
|
||||
for (TrainingCycle = 1; TrainingCycle < 2147483647; TrainingCycle++)
|
||||
{
|
||||
|
||||
/******************************************************************
|
||||
* Randomize order of training patterns
|
||||
******************************************************************/
|
||||
|
||||
for (p = 0; p < PatternCount; p++)
|
||||
{
|
||||
q = random(PatternCount);
|
||||
r = RandomizedIndex[p];
|
||||
RandomizedIndex[p] = RandomizedIndex[q];
|
||||
RandomizedIndex[q] = r;
|
||||
}
|
||||
Error = 0.0;
|
||||
/******************************************************************
|
||||
* Cycle through each training pattern in the randomized order
|
||||
******************************************************************/
|
||||
for (q = 0; q < PatternCount; q++)
|
||||
{
|
||||
p = RandomizedIndex[q];
|
||||
|
||||
/******************************************************************
|
||||
* Compute hidden layer activations
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < HiddenNodes; i++)
|
||||
{
|
||||
Accum = HiddenWeights[InputNodes][i];
|
||||
for (j = 0; j < InputNodes; j++)
|
||||
{
|
||||
Accum += Input[p][j] * HiddenWeights[j][i];
|
||||
}
|
||||
Hidden[i] = 1.0 / (1.0 + exp(-Accum));
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Compute output layer activations and calculate errors
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
Accum = OutputWeights[HiddenNodes][i];
|
||||
for (j = 0; j < HiddenNodes; j++)
|
||||
{
|
||||
Accum += Hidden[j] * OutputWeights[j][i];
|
||||
}
|
||||
Output[i] = 1.0 / (1.0 + exp(-Accum));
|
||||
OutputDelta[i] = (Target[p][i] - Output[i]) * Output[i] * (1.0 - Output[i]);
|
||||
Error += 0.5 * (Target[p][i] - Output[i]) * (Target[p][i] - Output[i]);
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Backpropagate errors to hidden layer
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < HiddenNodes; i++)
|
||||
{
|
||||
Accum = 0.0;
|
||||
for (j = 0; j < OutputNodes; j++)
|
||||
{
|
||||
Accum += OutputWeights[i][j] * OutputDelta[j];
|
||||
}
|
||||
HiddenDelta[i] = Accum * Hidden[i] * (1.0 - Hidden[i]);
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Update Inner-->Hidden Weights
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < HiddenNodes; i++)
|
||||
{
|
||||
ChangeHiddenWeights[InputNodes][i] = LearningRate * HiddenDelta[i] + Momentum * ChangeHiddenWeights[InputNodes][i];
|
||||
HiddenWeights[InputNodes][i] += ChangeHiddenWeights[InputNodes][i];
|
||||
for (j = 0; j < InputNodes; j++)
|
||||
{
|
||||
ChangeHiddenWeights[j][i] = LearningRate * Input[p][j] * HiddenDelta[i] + Momentum * ChangeHiddenWeights[j][i];
|
||||
HiddenWeights[j][i] += ChangeHiddenWeights[j][i];
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Update Hidden-->Output Weights
|
||||
******************************************************************/
|
||||
|
||||
for (i = 0; i < OutputNodes; i++)
|
||||
{
|
||||
ChangeOutputWeights[HiddenNodes][i] = LearningRate * OutputDelta[i] + Momentum * ChangeOutputWeights[HiddenNodes][i];
|
||||
OutputWeights[HiddenNodes][i] += ChangeOutputWeights[HiddenNodes][i];
|
||||
for (j = 0; j < HiddenNodes; j++)
|
||||
{
|
||||
ChangeOutputWeights[j][i] = LearningRate * Hidden[j] * OutputDelta[i] + Momentum * ChangeOutputWeights[j][i];
|
||||
OutputWeights[j][i] += ChangeOutputWeights[j][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* Every 1000 cycles send data to terminal for display
|
||||
******************************************************************/
|
||||
ReportEvery1000 = ReportEvery1000 - 1;
|
||||
if (ReportEvery1000 == 0)
|
||||
{
|
||||
|
||||
// Blink Led for State Info
|
||||
digitalWrite(ledPin, HIGH);
|
||||
delay(200);
|
||||
digitalWrite(ledPin, LOW);
|
||||
delay(200);
|
||||
digitalWrite(ledPin, HIGH);
|
||||
delay(200);
|
||||
digitalWrite(ledPin, LOW);
|
||||
delay(200);
|
||||
|
||||
DEBUG_PRINTLN("");
|
||||
DEBUG_PRINTLN("");
|
||||
DEBUG_PRINT_VALUE(" TrainingCycle", TrainingCycle);
|
||||
DEBUG_PRINT_VALUE(" Error Level", Error);
|
||||
|
||||
toTerminal();
|
||||
|
||||
if (TrainingCycle == 1)
|
||||
{
|
||||
ReportEvery1000 = 999;
|
||||
}
|
||||
else
|
||||
{
|
||||
ReportEvery1000 = 1000;
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************
|
||||
* If error rate is less than pre-determined threshold then end
|
||||
******************************************************************/
|
||||
|
||||
if (Error < Success)
|
||||
break;
|
||||
}
|
||||
DEBUG_PRINTLN("");
|
||||
DEBUG_PRINT_VALUE(" TrainingCycle: ", TrainingCycle);
|
||||
DEBUG_PRINT_VALUE(" Error Level", Error);
|
||||
|
||||
toTerminal();
|
||||
|
||||
DEBUG_PRINTLN("");
|
||||
DEBUG_PRINTLN("");
|
||||
DEBUG_PRINT("Training Set Solved!");
|
||||
DEBUG_PRINT_VALUE("TrainingCycle: ", TrainingCycle);
|
||||
DEBUG_PRINTLN_VALUE(" Error Level", Error);
|
||||
DEBUG_PRINTLN("");
|
||||
|
||||
ReportEvery1000 = 1;
|
||||
|
||||
// done=> set mode to run
|
||||
mode = 'c';
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Loop
|
||||
/******************************************************************************/
|
||||
void loop()
|
||||
{
|
||||
|
||||
// Train NN
|
||||
/******************************************************************************/
|
||||
if (mode == 't')
|
||||
{
|
||||
train_nn();
|
||||
}
|
||||
|
||||
// Check NN
|
||||
/******************************************************************************/
|
||||
if (mode == 'c')
|
||||
{
|
||||
check_nn();
|
||||
}
|
||||
|
||||
// Run NN
|
||||
/******************************************************************************/
|
||||
if (mode == 'r')
|
||||
{
|
||||
run_nn();
|
||||
}
|
||||
|
||||
// Analyze Sensors
|
||||
/******************************************************************************/
|
||||
if (mode == 'a')
|
||||
{
|
||||
analyze_sensors();
|
||||
}
|
||||
}
|
||||
14
test/README
Normal file
14
test/README
Normal file
@ -0,0 +1,14 @@
|
||||
|
||||
This directory is intended for PIO Unit Testing and project tests.
|
||||
|
||||
Unit Testing is a software testing method by which individual units of
|
||||
source code, sets of one or more MCU program modules together with associated
|
||||
control data, usage procedures, and operating procedures, are tested to
|
||||
determine whether they are fit for use. Unit testing finds problems early
|
||||
in the development cycle.
|
||||
|
||||
More information about PIO Unit Testing:
|
||||
- https://docs.platformio.org/page/plus/unit-testing.html
|
||||
|
||||
1
|
||||
22
|
||||
Reference in New Issue
Block a user