borgs.c

/*
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