Created
September 30, 2020 01:36
-
-
Save fakefarm/50aad7534ccde4841c27f8fa5da67f7e to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
Mmachine learnng crawling Robot using Simple Reinforcement Learning | |
by: jim demello | |
*/ | |
// Servo setup: the servos must be oriented so that if the arm is rotating counter-clockwise to the left of the servo, then up is 0 degrees | |
// and down is 180 degrees, for both servos. Then when the arm is in it's highest postion, servo 1 (the servo closest to the | |
// body of the robot, will be at 0 degrees and servo 2 will be at 40 degrees.) | |
// Sonar: the ultrasonic module should be placed facing the rear of the robot as it measures movement of the robot away from some | |
// solid structure like a wall. | |
// TODO: 1. smooth servo arm movements (simultaneous moves) - done | |
// 2. lower servo starting positions - done | |
// 3. try using wheel encoder rather than ultrasonic module for greater precision and so that robot does not have to | |
// measure distance from another object. | |
// goal: move between two arm positions that produce the greatest distance, without using arrays to store results. | |
// algorithm: this is a positive reinforcement unsupervised learning algorithm | |
// It chooses random servo positions for two arm positions - | |
// and then moves the arm from the first to the second position, gets distance moved and then if it is greater than | |
// 2cms, it keeps the arm positions that produce the greatest distance. | |
#include <Servo.h> | |
Servo servo1,servo2; | |
#ifdef _arm_ | |
// should use uinstd.h to define sbrk but Due causes a conflict | |
extern "C" char* sbrk(int incr); | |
#else // _ARM_ | |
extern char *__brkval; | |
#endif // _arm_ | |
int freeMemory() { // this routine reports on free ram space | |
char top; | |
#ifdef _arm_ | |
return &top - reinterpret_cast<char*>(sbrk(0)); | |
#elif defined(CORE_TEENSY) || (ARDUINO > 103 && ARDUINO != 151) | |
return &top - __brkval; | |
#else // _arm_ | |
return __brkval ? &top - __brkval : &top - __malloc_heap_start; | |
#endif // _arm_ | |
} | |
float distance; | |
float sonarTime; | |
int TRIGGER=7,ECHO=8; // sonar pins | |
//int state=0; | |
//int numberSuccesses = 0; | |
int episodes = 0; | |
int spos1 = 0; // servo1 position | |
int spos2 = 0; // servo2 position | |
int spos3 = 0; // servo1 position | |
int spos4 =0; // servo2 position | |
int spos1High =0; // servo1 highest position | |
int spos2High =0; | |
int spos3High =0; | |
int spos4High =0; | |
int distanceHigh=0; | |
float distDifference=0,distPrevious=0,distCurrent=0; | |
void setup (){ | |
Serial.begin(9600); | |
servo1.attach( 9, 600, 2400 ); | |
servo2.attach( 6, 600, 2400 ); | |
myServo(servo1,0,1,8,1); // set servos to zero position | |
delay(1000); | |
myServo(servo2,0,1,8,1); | |
delay(1000); | |
pinMode(TRIGGER, OUTPUT); // setup sonar | |
pinMode(ECHO, INPUT); | |
randomSeed(analogRead(0)); // get real random number seed | |
distPrevious = getDistance(); //get initial distance | |
Serial.println(distPrevious); | |
delay(1000); | |
// exit(0); // exit here to just test sonar | |
} // end setup | |
float getDistance() { // routine to measure distance = call and average it 5 times | |
int numberTriggers = 5; | |
int average = 0; | |
for(int i=0;i<numberTriggers;i++) { | |
digitalWrite(TRIGGER, LOW); | |
delayMicroseconds(5); | |
digitalWrite(TRIGGER, HIGH); | |
delayMicroseconds(10); | |
digitalWrite(TRIGGER, LOW); | |
sonarTime = pulseIn(ECHO, HIGH); | |
distance = sonarTime / 58.00; | |
average = average + distance; | |
// Serial.print(sonarTime);Serial.print(" ");Serial.print(distance);Serial.println("cm"); | |
delay(100); | |
} // end for i | |
average = average / numberTriggers; | |
Serial.println(average); | |
return average; | |
}// end get ultrasonic distance routine | |
void doTraining() { | |
Serial.println("Do doTraining..."); | |
episodes = 40; | |
for (int episode=0;episode<episodes;episode++) // for 10 episodes (even 5 episodes usually works) | |
{ | |
spos1 = random(160); | |
spos2 = random(160); | |
spos3 = random(160); | |
spos4 = random(160); | |
myServo(servo1,spos1,1,7,1); | |
myServo(servo2,spos2,1,7,1); | |
myServo(servo1,spos3,1,7,1); | |
myServo(servo2,spos4,1,7,1); | |
distCurrent = getDistance(); // get distance - note this is not always accurate so sometimes robot will just claw the air | |
distDifference = distCurrent - distPrevious; | |
distPrevious = distCurrent; | |
Serial.print(" episode = ");Serial.print(episode); | |
// Serial.print(" state = ");Serial.print(state); | |
Serial.print(" spos1 = ");Serial.print(spos1); | |
Serial.print(" spos2 = ");Serial.print(spos2); | |
Serial.print(" spos3 = ");Serial.print(spos3); | |
Serial.print(" spos4 = ");Serial.print(spos4); | |
Serial.print(" distance = ");Serial.println(distDifference); | |
if ( distCurrent > distanceHigh) { // if current distancee is greater than highest then replace postions | |
spos1High = spos1; // servo position 1 | |
spos2High = spos2; // servo position 2 | |
spos3High = spos3; // servo position 3 | |
spos4High = spos4; // servo position 4 | |
distanceHigh = distCurrent; | |
// distPrevious = distCurrent; | |
} | |
} // end each episode | |
Serial.print("spos1High = ");Serial.println(spos1High); | |
Serial.println(" "); | |
} // end doTraining | |
void doLearnedBehavior() { | |
Serial.println("Do Learned behavior... "); | |
myServo(servo1,0,1,8,1); | |
myServo(servo2,0,1,8,1); | |
delay(2000); | |
for (int i=0;i<30;i++) { | |
Serial.print(" spos1High= "); Serial.print(spos1High); | |
Serial.print(" spos2High = ");Serial.print(spos2High); | |
Serial.print(" spos3High = ");Serial.print(spos3High); | |
Serial.print(" spos4High = ");Serial.println(spos4High); | |
myServo(servo1,spos1High,1,7,1); | |
myServo(servo2,spos2High,1,7,1); | |
myServo(servo1,spos3High,1,7,1); | |
myServo(servo2,spos4High,1,7,1); | |
} // doLearned | |
} // end loop | |
void loop(){ // main loop reads success table and performs actions | |
int freespace = freeMemory(); Serial.print("free memory= "); Serial.println(freespace); | |
doTraining(); // trial and error training with distance reinforcement | |
freespace = freeMemory(); Serial.print("free memory= "); Serial.println(freespace); | |
doLearnedBehavior(); // do longest step 10 times to make robot crawl | |
myServo(servo1,0,1,8,1); | |
myServo(servo2,0,1,8,1); | |
Serial.print("end program "); | |
delay(2000); | |
exit(0); // quit program | |
} // end main loop | |
void myServo(Servo servo,int newAngle,int angleInc,int incDelay,int servoNum) { | |
int curAngle = 0; | |
curAngle = servo.read(); | |
if (curAngle < newAngle) { | |
for(int angle=curAngle;angle < newAngle;angle += angleInc) { | |
servo.write(angle); | |
delay(incDelay); } | |
} | |
else if (curAngle > newAngle) { | |
for(int angle=curAngle;angle > newAngle;angle -= angleInc) { | |
servo.write(angle); | |
delay(incDelay); } | |
} | |
} // end of myServo function |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment