Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// META PARAMETERS USED FOR TRAINING
// Generation: 143 of 200
// No of experiments per generation: 300
// Spot light displacement: 3.5
// Angular velocity punishment: 10000
// Runtime (episode duration in seconds): 4
// Initial weight max: 4
// Final weight max: 0.25
// Motor force multiplier: 10
// Number of Hidden nodes: 10
// Output activation function: NONE
// Include t minus one second: False
// INFO
// Number of generations survived: 14
const int InputNodes = 4;
const int HiddenNodes = 10;
const int OutputNodes = 2;
const float MotorForceMultiplier = 10;
const float InputScale = 10000;
float HiddenWeights[InputNodes+1][HiddenNodes] = {
{-10.5071 ,-28.6881 ,49.60723 ,-23.88357 ,-39.64715 ,-18.24572 ,-27.01153 ,-25.99557 ,15.14673 ,13.87427},
{1.652861 ,-13.10295 ,6.646737 ,7.469599 ,33.07862 ,-2.438618 ,-10.64128 ,18.80143 ,31.26506 ,-14.81889},
{21.20042 ,-13.02495 ,-11.88434 ,-12.08691 ,-23.28734 ,-34.93689 ,25.73363 ,-8.896099 ,-37.03425 ,-17.514},
{-19.2657 ,-10.00093 ,-13.0666 ,23.542 ,-0.7619491 ,-26.32526 ,50.7839 ,16.5293 ,4.468988 ,-20.49962},
{23.45566 ,19.99741 ,0.2138262 ,6.149847 ,6.500648 ,9.950813 ,-20.71023 ,31.42357 ,15.78693 ,27.31748}
};
float OutputWeights[HiddenNodes+1][OutputNodes] = {
{6.557656 ,1.594061},
{7.629223 ,-0.2973841},
{32.28601 ,27.33524},
{0.560276 ,-0.8649625},
{-48.71197 ,-2.280177},
{24.57121 ,-10.00805},
{-8.880691 ,-13.53585},
{-30.56511 ,-25.8589},
{-5.349677 ,-11.72226},
{19.98512 ,-1.323637},
{4.43928 ,25.86219}
};
float Input[InputNodes] = {0,0,0,0};
float Hidden[HiddenNodes];
float Output[OutputNodes] = {0,0};
/**
* H-bridge module HG7881CP/HG7881CP example code
* http://diyprojects.eu/how-to-use-h-bridge-hg7881-with-external-power-supply-and-arduino
*/
/**
* Create variables to be used to run motor A
*/
int motorAPin_A = 8; //Arduino digital 8 is connected to HG7881's A-1A terminal
int motorAPin_B = 9; //Arduino digital 9 is connected to HG7881's A-1B terminal
int motorBPin_A = 5; //Arduino digital 5 is connected to HG7881's A-1A terminal
int motorBPin_B = 6; //Arduino digital 6 is connected to HG7881's A-1B terminal
// ldr light sensor pins
int leftLightPin = A0;
int rightLightPin = A1;
int frontLightPin = A2;
int backLightPin = A3;
// ambient light level, to be subtracted, measured at start
int leftLightStart = 0;
int rightLightStart = 0;
int frontLightStart = 0;
int backLightStart = 0;
void setup(){
Serial.begin(9600);
/**
* When program starts set Arduino pinmode for 8 and 9 digital to be OUTPUT
* so we can use analogWrite to output values from 0 to 255 (0-5V) (PWM)
*/
pinMode(motorAPin_A, OUTPUT); //direction
pinMode(motorAPin_B, OUTPUT); //speed
pinMode(motorBPin_A, OUTPUT); //direction
pinMode(motorBPin_B, OUTPUT); //speed
pinMode(LED_BUILTIN, OUTPUT);
//read ambient light levels
leftLightStart = analogRead(leftLightPin);
rightLightStart = analogRead(rightLightPin);
frontLightStart = analogRead(frontLightPin);
backLightStart = analogRead(backLightPin);
}
void loop(){
stop(100);
blinkSpeed(20);
delay(10000); // wait a bit to disconnect from usb before starting motors
blinkSpeed(200);
int leftLight=0;
int rightLight=0;
int frontLight=0;
int backLight=0;
float leftv =0;
float rightv =0;
float frontv =0;
float backv =0;
float Accum;
int i, j;
// this is a small test to check if the motor wiring is correct
drive(155,0,400); // left wheel forward
delay(2000);
drive(-155,0,400); // left wheel backward
delay(2000);
drive(0,155,400); // right wheel forward
delay(2000);
drive(0,-155,400); // left wheel backward
delay(2000);
for (int i=0;i<2000;i++){
// read from light sensors, subtract ambient light
leftLight = analogRead(leftLightPin)-leftLightStart;
rightLight = analogRead(rightLightPin)-rightLightStart;
frontLight = analogRead(frontLightPin)-frontLightStart;
backLight = analogRead(backLightPin)-backLightStart;
leftv = leftLight / 512.0;
rightv = rightLight / 512.0;
frontv = frontLight / 512.0;
backv = backLight / 512.0;
//Serial.println("start");
//Serial.println(leftLight);
//Serial.println(rightLight);
//Serial.println(frontLight);
//Serial.println(backLight);
//Serial.println("end");
//delay(1000);
// feed to input layer of neural network
Input[0] = frontv*InputScale;
Input[1] = leftv*InputScale;
Input[2] = rightv*InputScale;
Input[3] = backv*InputScale;
// Compute hidden layer activations
for( i = 0 ; i < HiddenNodes ; i++ ) {
Accum = HiddenWeights[InputNodes][i] ;
for( j = 0 ; j < InputNodes ; j++ ) {
Accum += Input[j] * HiddenWeights[j][i] ;
}
// leaky ReLU
if (Accum<0) {
Hidden[i] = 0.1*Accum;
} else {
Hidden[i] = Accum;
}
//Hidden[i] = 1.0/(1.0 + exp(-Accum)) ;
}
// Compute output layer activations
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)) ;
// no activation function
Output[i] = Accum ;
}
// feed neural network output to motors
drive(Output[0]*MotorForceMultiplier/30000, Output[1]*MotorForceMultiplier/30000,50);
}
}
void drive(float leftMotor, float rightMotor, int time) {
//Serial.println("start");
//Serial.println(leftMotor);
//Serial.println(rightMotor);
int leftSpeed = min(abs(leftMotor),95);
int rightSpeed = min(abs(rightMotor),95);
//Serial.println("end");
if (leftMotor>0) {
analogWrite(motorBPin_A, 255);
analogWrite(motorBPin_B, invertOurValue( leftSpeed ) );
} else {
analogWrite(motorBPin_A, LOW);
analogWrite(motorBPin_B, leftSpeed);
}
if (rightMotor>0) {
analogWrite(motorAPin_A, 255);
analogWrite(motorAPin_B, invertOurValue( rightSpeed ) );
} else {
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, rightSpeed);
}
delay(time);
}
void stop(int delayAfterStop){
// set both direction and pwm to low
analogWrite(motorAPin_A, LOW);
analogWrite(motorAPin_B, LOW);
analogWrite(motorBPin_A, LOW);
analogWrite(motorBPin_B, LOW);
delay(delayAfterStop);
}
void blink(){
for(int i=0; i<=5; i++){
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
delay(100); // wait for a second
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
delay(100);
}
}
void blinkSpeed(int speed){
for(int i=0; i<=5; i++){
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
delay(255-speed); // wait for a second
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
delay(255-speed);
}
}
int invertOurValue(int input) {
return 255 - input;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.