Het Robot Project

Maart 2018:
Een aantal weken geleden repareerde ik een robotstofzuiger. Die robot kon zelf zijn weg terug vinden naar het laadstation. Dat inspireerde mij om zelf een nieuwe robot te bouwen, geen stofzuiger maar wel een die zelf de weg naar het laadstation zou kunnen vinden.

Dit is de robot tot nu toe, het project is nog niet afgerond list

Het Schema

Dit is het schema van de robot uit de tweede video.

Klik om te vergroten!

In het schema heb ik niet alle voedingslijnen naar de sensors getekend, die zijn er in het echt natuurlijk wel.

Onderdelen

Arduino Mega

Ik maak gebruik van een Arduino Mega. Die koop je op ebay (in China) of gewoon in Nederland bij opencircuit.nl.

Motor Controller

De kleine motorcontroller uit de eerste video heeft de geest gegeven, dus gebruik ik nu een zwaarder model: L298N motor driver

Batterijen en houders

De batterijen zijn 18650 cellen, deze hebben een spanning van bijna 4 volt per stuk. Ik gebruik er 3 in serie om op 12 volt te komen

De batterijhouders zijn 3 losse houders. Ik maak dus geen gebruik van een enkele drievoudige houder, waarom? dat wordt later duidelijk

Sensoren

De robot heeft (tot nu toe) drie ultra sone afstandssensoren.
HC-SR04

Micro switches

De bumpers maken gebruik van deze goedkope, kleine, microswitches

Motors

De motortjes komen van ebay. Ze bestaan in verschillende snelheden, ik gebruik de 100rpm variant.

Rupsbanden

De rupsbanden kocht ik op ebay, ze zijn niet altijd goed te vinden dus ik hoop dat deze link werkt

Onderdelen uit de 3d printer

Een aantal onderdelen maal ik zelf met de 3d printer. Ik print alles met PLA, met een nozzel van 0.6mm en een layerhoogte van 0.4 (Zeer grof dus)
Hier zijn de stl files zodat je ze zelf kunt printen. Klik op de afbeelding om het onderdeel te downloaden.

Onderdeel	Afbeelding (download link)	Datum
Frame		Maart 2018
Wielen (2 sets nodig)		Maart 2018
Motorbeugels		Maart 2018
Bumpers		Maart 2018
sensorhouder		Maart 2018
Arduinohouder		Maart 2018

Het Onderstel

In de eerste video ontwerp en bouw ik het onderstel

De robot krijgt sensoren

In de tweede video monteer ik de sensoren

Arduino Sketch

Hier is de arduino sketch (uit de tweede video!) voor de Arduino Mega 2560

// Robot op rupsbanden
// Bart Venneker 2018
// www.bartvenneker.nl

// Motor controller
#define motorD1 4 // 1
#define motorS1 5 // 2
#define motorS2 6 // 3
#define motorD2 7 // 4

// Sensors
// Er zijn 3 ultrasone sensors, de trigger pins zijn allemaal verbonden met pin 9
// De echo pins gaan naar 8,10 en 11
#define trig 9 
#define echoL 10
#define echoM 8
#define echoR 11
unsigned int distance[3]; // array om de 3 afstanden op te slaan

// Er zijn 2 bumper sensoren (gewoon schakelaars), linksvoor en rechtsvoor
#define bumberR 19
#define bumberL 18
int bumperhit=0;

void setup() {

  pinMode(motorD1, OUTPUT); digitalWrite(motorD1,LOW);
  pinMode(motorS1, OUTPUT); digitalWrite(motorS1,LOW);
  pinMode(motorD2, OUTPUT); digitalWrite(motorD2,LOW);
  pinMode(motorS2, OUTPUT); digitalWrite(motorS2,LOW);

  pinMode(trig, OUTPUT); digitalWrite(trig,LOW);
  pinMode(echoM, INPUT); 
  pinMode(echoR, INPUT); 
  pinMode(echoL, INPUT); 

  pinMode(bumberL, INPUT_PULLUP);
  pinMode(bumberR, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(bumberL), bumper,FALLING );
  attachInterrupt(digitalPinToInterrupt(bumberR), bumper,FALLING );
      
  delay(3000); // pauze van 3 sec
   
}

void loop() {
if (false) {
  if ( bumperhit > 0 )  {
     ontwijk(bumperhit);
     bumperhit = 0;
     }
  else {
    driveTracks(3,255,0);  
    Sensordata(0);
    if (distance[1] < 25) {      
        Sensordata(1);
        if (distance[0] > distance[2]) {
          driveTracks(0,0,0);
          driveTracks(1,255,0);
          delay(1000);
          } else {
          driveTracks(0,0,0);
          driveTracks(2,255,0);
          delay(1000);
          }
      }
  }
}
  
}


void Sensordata(int i){
  // i=0 mid sensor only
  // i > 0 all sensors

  // eerst de middelste sensor
  digitalWrite(trig,HIGH); delayMicroseconds(8); digitalWrite(trig,LOW); delayMicroseconds(3);
  distance[1] = pulseIn(echoM, HIGH, 38000) / 58;
  delay(7);

  // als i nul is stoppen we ermee
  if (i==0) return;
  
  // nu de andere twee sensoren
  digitalWrite(trig,HIGH); delayMicroseconds(8); digitalWrite(trig,LOW); delayMicroseconds(3);
  distance[0] = pulseIn(echoL, HIGH, 38000) / 58;
  delay(7);
  
  digitalWrite(trig,HIGH); delayMicroseconds(8); digitalWrite(trig,LOW); delayMicroseconds(3);
  distance[2] = pulseIn(echoR, HIGH, 38000) / 58;
  delay(7);
}


void bumper(){
  // deze functie gaat af zodra de bumpers geraakt worden
  if (bumperhit == 0 )driveTracks(0,0,0);  
  if (!digitalRead(bumberL)) bumperhit=1;
  if (!digitalRead(bumberR)) bumperhit=2;
  }


void ontwijk(int w){
  // dit is de ontwijk procedure voor de bumpers
  driveTracks(0,0,0);  
  // achteruit
  driveTracks(3,255,1);  
  delay(600);
  driveTracks(w,0,0);  
  delay(600);
  driveTracks(0,0,0);  
  }

 
void driveTracks(int belt,int s,bool d) {
  // d = direction
  // s = speed (0~255)
  // belt :
  // 0 = stop all
  // 1 = belt 1
  // 2 = belt 2
  // 3 = both
  if (d) { s=255-s ; }
  
  switch (belt) {
    case 0:   // stop
      digitalWrite(motorD1,0);
      digitalWrite(motorS1,0);
      digitalWrite(motorD2,0);
      digitalWrite(motorS2,0);
      break;
    case 1: // only one track    
      digitalWrite(motorD1,d);
      analogWrite (motorS1,s);
      break;
    case 2: // only the other track       
      digitalWrite(motorD2,d);
      analogWrite (motorS2,s);
      break;
    case 3: // both tracks    
      digitalWrite(motorD1,d);
      analogWrite (motorS1,s); 
      digitalWrite(motorD2,d);
      analogWrite (motorS2,s);
      break;
  }
  
}

De derde video

De vierde video

Hier is de arduino sketch (uit de vijfde video!) voor het basis station

#include <PinChangeInt.h> // PinChange Interrupt library


String h="0";
// myId = 1 ~ 254
#define myId 201
#define ledL A5
#define ledR 12
#define buttonIN  8
#define buttonOUT 9
// Buffer to hold the message (received by serial connection)
char serialMessage[50];
char bufferMessage[50];
unsigned long pingTimer=0;
bool waiting = 0;
int pulsecountR=0;
int pulsecountL=0;
int pulsecountwest=0;
int pulsecounteast=0;


void setup() {
    // put your setup code here, to run once:
  pinMode(2,INPUT_PULLUP);
  pinMode(3,INPUT_PULLUP);  
  pinMode(4,INPUT_PULLUP);
  pinMode(5,INPUT_PULLUP);
  attachPinChangeInterrupt(2, trig1, FALLING); // interrupt connected to pin 2
  attachPinChangeInterrupt(3, trig2, FALLING); // interrupt connected to pin 3
  attachPinChangeInterrupt(4, trig3, FALLING); // interrupt connected to pin 4
  attachPinChangeInterrupt(5, trig4, FALLING); // interrupt connected to pin 5
    
  pinMode(buttonIN,INPUT_PULLUP);
  pinMode(buttonOUT,INPUT_PULLUP);
  pinMode(ledR,OUTPUT);
  pinMode(ledL,OUTPUT);
  digitalWrite(ledR,HIGH);
  digitalWrite(ledL,HIGH);
  
  Serial.begin(9600);
  
  delay(500);
  digitalWrite(ledR,LOW);    
  digitalWrite(ledL,LOW);
}

bool getMessage(){
  static char *cp = bufferMessage;
  bool idMatch = false;
  if (!Serial.available()){
    return false;
  }
  
  // the first byte should match our ID
  if(Serial.available()){
    int thisid = (int)Serial.read();
    if (thisid == myId) idMatch=true;    
    delay(3);
  }
  
  while (Serial.available()) {
    *cp=(char)Serial.read();
    delay(3);
    if ((*cp == '\n') || (*cp == '\r') ||(cp - bufferMessage >= 48)) // end of message character or full buffer
    {
      *cp='\0'; // end of string character
      cp=bufferMessage; 
      if (idMatch) strcpy(serialMessage,bufferMessage);
      return idMatch;     
    }
    else {
      cp++;      
    }
    
  }
  
  
}

void trig1(){
  
  if (millis() - pingTimer > 50) { waiting=false ;return;}     
  pulsecountR++;  
  if (h=="0" && waiting) {    
    h="R";
    digitalWrite(ledL,LOW);
    digitalWrite(ledR,HIGH);
    waiting=false;
    
  }
  
}

void trig2(){
  
  if (millis() - pingTimer > 50) { waiting=false ;return;}  
  pulsecountL++;
  if (h=="0" && waiting){
    h="L";
    digitalWrite(ledR,LOW);
    digitalWrite(ledL,HIGH);
    waiting=false;
    
  }
}

void trig3(){
  
  if (millis() - pingTimer > 50) { waiting=false ;return;}  
  pulsecounteast++;
  if (h=="0" && waiting){
    h="E";
    digitalWrite(ledR,LOW);
    digitalWrite(ledL,HIGH);
    waiting=false;
    
  }
}

void trig4(){
  
  if (millis() - pingTimer > 50) { waiting=false ;return;}  
  pulsecountwest++;
  if (h=="0" && waiting){
    h="W";
    digitalWrite(ledR,LOW);
    digitalWrite(ledL,HIGH);
    waiting=false;
    
  }
}
void loop() {
  int pc=0;
  if (getMessage()){
        
    if (serialMessage[0]=='P'){      
      pingTimer=millis();  
      waiting=1; 
      pulsecountL=0; 
      pulsecountR=0; 
      pulsecountwest=0;
      pulsecounteast=0;
      delay(30);    

    }
    
    if (serialMessage[0]=='H') {          
      if (serialMessage[1]=='1'){
        digitalWrite(ledR,HIGH);    
        digitalWrite(ledL,HIGH);        
      } else {
        digitalWrite(ledR,LOW);    
        digitalWrite(ledL,LOW);        
      }
      
    }  
    
    
    serialMessage[0]='\0';
  }
  
  if (h!="0") {  
    pc=(pulsecountL)+32; if (pc>125) pc=125;
    char pulseCountLChar=pc; // pulse count as character for easy transfer

    pc=(pulsecountR)+32; if (pc>125) pc=125;
    char pulseCountRChar=pc; // pulse count as character for easy transfer
    
    pc=(pulsecounteast)+32; if (pc>125) pc=125;
    char pulseCountEastChar=pc;

    pc=(pulsecountwest)+32; if (pc>125) pc=125;
    char pulseCountWestChar=pc;
    
    sendMessage(200,h + pulseCountLChar + pulseCountRChar + pulseCountEastChar + pulseCountWestChar) ;
    
    delay(50);
    digitalWrite(ledR,LOW);    
    digitalWrite(ledL,LOW); 
    delay(50);
    h="0";     
    }

  if (!digitalRead(buttonIN)) {
    // wait to release the button
    delay(1000);
    // Call the robot to come home! 
    sendMessage(200,"H") ;     
  }
  
  
  if (!digitalRead(buttonOUT)) {    
    // Send the robot away
    sendMessage(200,"Q") ; 
    digitalWrite(ledR,LOW);    
    digitalWrite(ledL,LOW);        
    delay(100);
  }
}

void sendMessage(int id,String message){
  Serial.write(id);
  Serial.println(message);  
}

void clearRxBuffer(){
  char d;
  while (Serial.available()) {
    d=(char)Serial.read();
  delay(3);
  }
}

Hier is de sketch voor de robot uit de vijfde video:

// Robot op rupsbanden
// Bart Venneker 2018
// www.bartvenneker.nl


// Motor controller
#define motorD1 4 // 1
#define motorS1 5 // 2
#define motorS2 6 // 3
#define motorD2 7 // 4

// afstandssensoren
// Er zijn 3 ultrasone sensors, de trigger pins zijn 9,52 en 53
// De echo pins gaan naar 8,10 en 11
#define trigM 9 
#define trigL 52 
#define trigR 53
#define echoL 10
#define echoM 8
#define echoR 11
unsigned int distance[3]; // array om de 3 afstanden op te slaan
unsigned long drivetimer;

// Er zijn 2 bumper sensoren (gewoon schakelaars), linksvoor en rechtsvoor
#define bumberR 19
#define bumberL 18
int bumperhit=0;

// id for serial communication, myId = 1 ~ 254
#define myId 200

char serialMessage[50];
char bufferMessage[50];
char sensors[4] = "LREW";  // translation array for the sensor names (Left Right East West)

bool drive=false;
bool reversing=false;
bool charging=false;
bool onFinalApproach=false;
bool homing =false;
bool avoided=false;

int signalStrength[4]; // array to hold the signal strength
int bestsignals[4];
int bestsignalsensor=0;
int bestsignalstrength=0;
int homingStage =0;

void setup() {

  pinMode(motorD1, OUTPUT); digitalWrite(motorD1,LOW);
  pinMode(motorS1, OUTPUT); digitalWrite(motorS1,LOW);
  pinMode(motorD2, OUTPUT); digitalWrite(motorD2,LOW);
  pinMode(motorS2, OUTPUT); digitalWrite(motorS2,LOW);

  pinMode(trigM, OUTPUT); digitalWrite(trigM,LOW);
  pinMode(trigL, OUTPUT); digitalWrite(trigL,LOW);
  pinMode(trigR, OUTPUT); digitalWrite(trigR,LOW);
  pinMode(echoM, INPUT); 
  pinMode(echoR, INPUT); 
  pinMode(echoL, INPUT); 

  pinMode(bumberL, INPUT_PULLUP);
  pinMode(bumberR, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(bumberL), bumper,FALLING );
  attachInterrupt(digitalPinToInterrupt(bumberR), bumper,FALLING );
      
  delay(3000); // pauze van 3 sec
  
  Serial3.begin(9600); // Xbee
  Serial.begin(9600); // USB for serial monitor (debug messages)
  Serial.println("Robot project 2018, Bart Venneker");  

if (false) {
  findDirection(14);
  delay(500);
  findDirection(5);

while (true) {}  
}


}


void loop() {

  if (charging){
    if (digitalRead(bumberL) || digitalRead(bumberR)) {        
      sendMessage(201,"H0") ;
      charging=false;
    }
  }
  
  if(getMessage()){
    if (serialMessage[0]=='H') {homing=true;} // Received message to come home
    if (serialMessage[0]=='Q') {homing=false;onFinalApproach=false;moveOut();}  // Received message to move out
    serialMessage[0]='\0'; // empty the serial message
    clearRxBuffer(); // empty the recieve buffer
  }

  if (homing) goHome();  
  
  if (drive)  driveAndAvoidObstacles();
  
}

/* *************************************************************************************
 *                      Functions for moving and driving
 * *************************************************************************************
 */

void driveAndAvoidObstacles(){
  if ( bumperhit > 0 )  {    
    reverseTurn(bumperhit);
    bumperhit = 0;
    avoided=true;
  } else {
    driveTracks(3,255,0);  
    Sensordata(0);
    if (distance[1] < 25) {      
      Sensordata(1);
      avoided=true;
      if (distance[0] > distance[2]) 
        driveTracks(2,0,0);
      else 
        driveTracks(1,0,0);
      delay(1000);      
    }
  }
}

void turnR(int d){  
  driveTracks(1,255,1);
  driveTracks(2,255,0);
  delay(d);
  driveTracks(0,0,0);
}

void turnL(int d){
  driveTracks(1,255,0);
  driveTracks(2,255,1);
  delay(d);
  driveTracks(0,0,0);  
}

void crawl(int d){
  driveTracks(0,0,0);
  driveTracks(d,200,0); 
  delay(100);
  driveTracks(0,0,0);
}
  
void driveDistance(int lengthCM, int dir){
  // dir=direction, 0=forward, 1 =reverse
  // snelheid = 24cm per seconde
  // dus  1cm = 41 mS

  if (dir==1) reversing=true;
  Serial.print("driving (cm) ");
  Serial.println(lengthCM);
  // convert cm to milliseconds
  int x =(lengthCM > 5) ? 41 * lengthCM-6 : 41 * lengthCM;
  unsigned long drivetime = millis() + x;
  avoided=false;
  bumperhit=0;
  while (millis() < drivetime and !avoided) {
    if ( bumperhit > 0 )  {
      Serial.println("Bumper hit!");
      reversing=true;
      driveTracks(3,255,1);
      bumperhit = 0;
      delay(1000);
      driveTracks(0,0,0);      
      avoided=true;      
    } else {
      driveTracks(3,255,dir); 
    }
  }
  reversing=false;  
}

void moveOut(){
  if (drive) return;
  // reverse
  reversing=true;
  driveTracks(3,255,1);  
  delay(750);
  reverseTurn(2);
  reversing=false;
  drive=true;
  bumperhit=0;
}

void reverseTurn(int w){
  // Make a reverse turn
  driveTracks(0,0,0);  
  driveTracks(3,255,1);  
  delay(600);
  driveTracks(w,0,0);  
  delay(600);
  driveTracks(0,0,0);  
  }

 
void driveTracks(int belt,int s,bool d) {
  // d = direction
  // s = speed (0~255)
  // belt :
  // 0 = stop all
  // 1 = belt 1
  // 2 = belt 2
  // 3 = both
  if (d) { s=255-s ; }
  
  switch (belt) {
    case 0:   // stop
      digitalWrite(motorD1,0);
      digitalWrite(motorS1,0);
      digitalWrite(motorD2,0);
      digitalWrite(motorS2,0);
      break;
    case 1: // only one track    
      digitalWrite(motorD1,d);
      analogWrite (motorS1,s);
      break;
    case 2: // only the other track       
      digitalWrite(motorD2,d);
      analogWrite (motorS2,s);
      break;
    case 3: // both tracks    
      digitalWrite(motorD1,d);
      analogWrite (motorS1,s); 
      digitalWrite(motorD2,d);
      analogWrite (motorS2,s);
      break;
  }
  
}


/* *************************************************************************************
 *               Functions for (XBEE) communication with the charger
 * *************************************************************************************
 */
 bool getMessage(){
  static char *cp = bufferMessage;
  bool idMatch = false;
  if (!Serial3.available()){
    return false;
  }
  
  // the first byte should match our ID
  if(Serial3.available()){
    int thisid = (int)Serial3.read();
    if (thisid == myId) idMatch=true;    
    delay(4);
  }
  
  while (Serial3.available()) {
    *cp=(char)Serial3.read();
    delay(4);
    
    if ((*cp == '\n') || (cp - bufferMessage >= 48)) // end of message character or full buffer
    {
      *cp=0; // end of string character
      cp=bufferMessage;
      if (idMatch) strcpy(serialMessage,bufferMessage);      
      return idMatch;
      break;
    }
    else {
      cp++;      
    }
    
  }
  
}

void sendMessage(int id,String message){
  Serial3.write(id);
  Serial3.println(message);
}
void clearRxBuffer(){
  char d;
  while (Serial3.available()) {
    d=(char)Serial3.read();
    delay(3);
  }
}


/* *************************************************************************************
 *                                   Bumpers and sensors
 * *************************************************************************************
 */

 void Sensordata(int i){
  // i=0 mid sensor only
  // i > 0 all sensors

  // First get the middle sensor
  digitalWrite(trigM,HIGH); delayMicroseconds(8); digitalWrite(trigM,LOW); delayMicroseconds(3);
  distance[1] = pulseIn(echoM, HIGH,38000) / 58;
  delay(7);

  if (i==0) return; // Return if i ==0
  
  // Now do the other 2 sensors
  digitalWrite(trigL,HIGH); delayMicroseconds(8); digitalWrite(trigL,LOW); delayMicroseconds(3);
  distance[0] = pulseIn(echoL, HIGH, 38000) / 58;
  delay(7);
  
  digitalWrite(trigR,HIGH); delayMicroseconds(8); digitalWrite(trigR,LOW); delayMicroseconds(3);
  distance[2] = pulseIn(echoR, HIGH, 38000) / 58;
  delay(7);
}

void bumper(){
  // This void is triggered when the bumbers are hit
  if (!reversing && !onFinalApproach) {  
    if (bumperhit == 0 )driveTracks(0,0,0);  
    if (!digitalRead(bumberL)) bumperhit=1;
    if (!digitalRead(bumberR)) bumperhit=2;
  }
}

/* *************************************************************************************
 *                               Functions for Homing
 * *************************************************************************************
 */

boolean pingMidSensor(){  
  // ping the charging station.
  // the charging station will send the signal strengths back
  clearSignalStrength();
  sendMessage(201,"P1") ;
  delay(50);
   
  digitalWrite(trigM,HIGH); 
  delayMicroseconds(8); 
  digitalWrite(trigM,LOW);
  delay(7);
   
  delay(200);  
  if(getMessage()){
    if (serialMessage[0]=='L' || serialMessage[0]=='R' || serialMessage[0]=='E' || serialMessage[0]=='W') {
      signalStrength[0] = (int)serialMessage[1]-32;    // Left      
      signalStrength[1] = (int)serialMessage[2]-32;    // Right      
      signalStrength[2] = (int)serialMessage[3]-32;    // East      
      signalStrength[3] = (int)serialMessage[4]-32;    // West      
    }          
    
  } else return false;
  
  return true;
}

void reportSignalStrength(){
  Serial.print("LEFT="); Serial.print(signalStrength[0]);
  Serial.print(", RIGH="); Serial.print(signalStrength[1]);
  Serial.print(", EAST="); Serial.print(signalStrength[2]);
  Serial.print(", WEST="); Serial.println(signalStrength[3]);
  
}

void clearSignalStrength(){
  for (int c=0; c<5 ;c++) signalStrength[c] =0;
}

void goHome(){
    if (homingStage==0)findDirection(14); 
    homingStage=1;
    if (!homing) return;
    findDirection(5); 
    if (!homing) return;    
    pingMidSensor();
    if (chargerAdhead()) finalApproach();
    if (!homing) return;
    
    Sensordata(0);
    if (bestsignalstrength > 28) 
      driveDistance(10,0);
    else {
      int dis = (int)distance[1]/2;
      dis = (dis > 60)? 60 : dis;  // maximize distance to 60cm when homing
      driveDistance(dis,0);  
    }
    if (avoided) homingStage=0;
}


void finalApproach(){
  unsigned long t;
  Serial.println("Final Approach");
  onFinalApproach=1;
  homing=false;
  while (digitalRead(bumberL) || digitalRead(bumberR)) {
    pingMidSensor();
    if (serialMessage[0]=='L') crawl(1);
    if (serialMessage[0]=='R') crawl(2);
    serialMessage[0]='\0';
      
    if (!digitalRead(bumberL)) crawl(1);
    if (!digitalRead(bumberR)) crawl(2);
  }
  sendMessage(201,"H1") ;
  drive=false;
  delay(500);
  charging=true;  
}

void findDirection360(){  
  driveTracks(0,0,0);
  if (charging) return;  
  int bestangle=0;
  bestsignalstrength=0;

  // Turn around and messure 14 angles to find the charging station.
  for (int x=1; x<14;x++){    
    //Serial.print("x = ");Serial.print(x);Serial.print(" ");
    pingMidSensor();
    if (chargerAdhead()) {finalApproach(); return;} 
    // we receive 4 values (L,R,E,W), find out which was the strongest
    
    for (int w=0;w<4;w++){      
      if (signalStrength[w] > bestsignalstrength) {
        bestsignalstrength=signalStrength[w];
        bestsignalsensor=w;
        bestangle = x;
        memcpy( bestsignals, signalStrength, 4*sizeof(int) );        
      }
    
    }
    //Serial.println();    
    delay(50);
    // draai een beetje en ping nogmaals
    turnL(250);
    driveTracks(0,0,0);
  }
  
  // turn back to best angle
  if (bestangle < 6) 
    for (int x=1; x<bestangle;x++) turnL(250);
  else 
    for (int x=14; x>bestangle;x--) turnR(250);       
    
  driveTracks(0,0,0);
 
}

void findDirection(int steps){  
  // steps = 14 or 5
  Serial.println();
  if (steps > 7) Serial.println("find direction 360");
  if (steps < 7) Serial.println("find direction 90");
  driveTracks(0,0,0);
  if (charging) return;  
  int bestangle=0;
  bestsignalstrength=0;
  if (steps < 7)  turnR(500);
  // Turn around and messure 14 angles to find the charging station.
  for (int x=1; x<steps;x++){    
    //Serial.print("x = ");Serial.print(x);Serial.print(" ");
    pingMidSensor();
    reportSignalStrength();
    if (chargerAdhead()) {finalApproach(); return;} 
    // we receive 4 values (L,R,E,W), find out which was the strongest
    
    for (int w=0;w<4;w++){      
      if (signalStrength[w] > bestsignalstrength) {
        bestsignalstrength=signalStrength[w];
        bestsignalsensor=w;
        bestangle = x;
        memcpy( bestsignals, signalStrength, 4*sizeof(int) );        
      }
    
    }    
    // draai een beetje en ping nogmaals
    delay(50);
    turnL(250);
    driveTracks(0,0,0);
  }
  
  Serial.print("BestAngle="); Serial.println(bestangle);
  // turn back to best angle
  if (bestangle==0) {    
    Serial.print("No direction found");
    return;
  }
  
  if (steps > 7 && bestangle < 6) {
    int leftTurns=bestangle-1;
    turnL(250 * leftTurns);
    Serial.print("Left turns="); Serial.println(leftTurns);    
  }
  else {
    int rightTurns=(steps-1)-bestangle;
    turnR(250 * rightTurns);     
    Serial.print("Right turns="); Serial.println(rightTurns);
    }
      
  memcpy(signalStrength,bestsignals,  4*sizeof(int)); 
  if (chargerAtEast()) {
    turnL(250); 
    Serial.println("Correcting course (east)");
  }
}

void findDirection90(){  
  driveTracks(0,0,0);
  if (charging) return;    
  int bestangle=0;
  bestsignalstrength=0;
  turnR(500);
  // Turn around and messure only 4 angles to find the charging station.
  for (int x=1; x<5;x++){       
    pingMidSensor();
    if (chargerAdhead()) {finalApproach(); return;} 
    // we receive 4 values (L,R,E,W), find out which was the strongest    
    for (int w=0;w<4;w++){      
      if (signalStrength[w] > bestsignalstrength) {
        bestsignalstrength=signalStrength[w];
        bestsignalsensor=w;
        bestangle = x;
        memcpy( bestsignals, signalStrength, 4*sizeof(int) );        
      }
    
    }  
    delay(50);
    // draai een beetje en ping nogmaals
    turnL(250);
  }
  
  Serial.print("Best set: ");
  for (int e=0;e<4;e++) {
    Serial.print (sensors[e]); Serial.print("="); Serial.print(bestsignals[e]);Serial.print(" ");
  }
  Serial.println();
    
  for (int x=4; x>bestangle;x--) turnR(250);    
   
  driveTracks(0,0,0);
  memcpy(signalStrength,bestsignals,  4*sizeof(int)); 
  if (chargerAtEast()) {
    turnL(250); 
    Serial.println("Correcting course (east)");
  }
}


bool chargerAdhead() {
  if ((signalStrength[0] > 30 || signalStrength[1] > 30) && signalStrength[0] + signalStrength[1] >50)  return true;
  return false;
}

bool chargerAtEast() {  
  if (signalStrength[0] < 10 && signalStrength[1] < 10 && signalStrength[2] > 20) return true;  
  return false;
}

Code om de sensors te testen

Hier is de sketch uit de 5e video voor de robot met een extra test functie voor de sensors.
Bij het opstarten van de robot worden eerst de sensors getest, het resultaat wordt naar de serial monitor gestuurd, de baud rate is 9600

// Robot op rupsbanden
// Bart Venneker 2018
// www.bartvenneker.nl


// Motor controller
#define motorD1 4 // 1
#define motorS1 5 // 2
#define motorS2 6 // 3
#define motorD2 7 // 4

// afstandssensoren
// Er zijn 3 ultrasone sensors, de trigger pins zijn 9,52 en 53
// De echo pins gaan naar 8,10 en 11
#define trigM 9
#define trigL 52
#define trigR 53
#define echoL 10
#define echoM 8
#define echoR 11
unsigned int distance[3]; // array om de 3 afstanden op te slaan
unsigned long drivetimer;

// Er zijn 2 bumper sensoren (gewoon schakelaars), linksvoor en rechtsvoor
#define bumberR 19
#define bumberL 18
int bumperhit = 0;

// id for serial communication, myId = 1 ~ 254
#define myId 200

char serialMessage[50];
char bufferMessage[50];
char sensors[4] = "LREW";  // translation array for the sensor names (Left Right East West)

bool drive = false;
bool reversing = false;
bool charging = false;
bool onFinalApproach = false;
bool homing = false;
bool avoided = false;

int signalStrength[4]; // array to hold the signal strength
int bestsignals[4];
int bestsignalsensor = 0;
int bestsignalstrength = 0;
int homingStage = 0;

void setup() {

  pinMode(motorD1, OUTPUT); digitalWrite(motorD1, LOW);
  pinMode(motorS1, OUTPUT); digitalWrite(motorS1, LOW);
  pinMode(motorD2, OUTPUT); digitalWrite(motorD2, LOW);
  pinMode(motorS2, OUTPUT); digitalWrite(motorS2, LOW);

  pinMode(trigM, OUTPUT); digitalWrite(trigM, LOW);
  pinMode(trigL, OUTPUT); digitalWrite(trigL, LOW);
  pinMode(trigR, OUTPUT); digitalWrite(trigR, LOW);
  pinMode(echoM, INPUT);
  pinMode(echoR, INPUT);
  pinMode(echoL, INPUT);

  pinMode(bumberL, INPUT_PULLUP);
  pinMode(bumberR, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(bumberL), bumper, FALLING );
  attachInterrupt(digitalPinToInterrupt(bumberR), bumper, FALLING );

  delay(3000); // pauze van 3 sec

  Serial3.begin(9600); // Xbee
  Serial.begin(9600); // USB for serial monitor (debug messages)
  Serial.println("Robot project 2018, Bart Venneker");

  SensorTest();
}


void loop() {

  if (charging) {
    if (digitalRead(bumberL) || digitalRead(bumberR)) {
      sendMessage(201, "H0") ;
      charging = false;
    }
  }

  if (getMessage()) {
    if (serialMessage[0] == 'H') {
      homing = true; // Received message to come home
    }
    if (serialMessage[0] == 'Q') {
      homing = false;  // Received message to move out
      onFinalApproach = false;
      moveOut();
    }
    serialMessage[0] = '\0'; // empty the serial message
    clearRxBuffer(); // empty the recieve buffer
  }

  if (homing) goHome();

  if (drive)  driveAndAvoidObstacles();

}

/* *************************************************************************************
                        Functions for moving and driving
 * *************************************************************************************
*/

void driveAndAvoidObstacles() {
  if ( bumperhit > 0 )  {
    reverseTurn(bumperhit);
    bumperhit = 0;
    avoided = true;
  } else {
    driveTracks(3, 255, 0);
    Sensordata(0);
    if (distance[1] < 25) {
      Sensordata(1);
      avoided = true;
      if (distance[0] > distance[2])
        driveTracks(2, 0, 0);
      else
        driveTracks(1, 0, 0);
      delay(1000);
    }
  }
}

void turnR(int d) {
  driveTracks(1, 255, 1);
  driveTracks(2, 255, 0);
  delay(d);
  driveTracks(0, 0, 0);
}

void turnL(int d) {
  driveTracks(1, 255, 0);
  driveTracks(2, 255, 1);
  delay(d);
  driveTracks(0, 0, 0);
}

void crawl(int d) {
  driveTracks(0, 0, 0);
  driveTracks(d, 200, 0);
  delay(100);
  driveTracks(0, 0, 0);
}

void driveDistance(int lengthCM, int dir) {
  // dir=direction, 0=forward, 1 =reverse
  // snelheid = 24cm per seconde
  // dus  1cm = 41 mS

  if (dir == 1) reversing = true;
  Serial.print("driving (cm) ");
  Serial.println(lengthCM);
  // convert cm to milliseconds
  int x = (lengthCM > 5) ? 41 * lengthCM - 6 : 41 * lengthCM;
  unsigned long drivetime = millis() + x;
  avoided = false;
  bumperhit = 0;
  while (millis() < drivetime and !avoided) {
    if ( bumperhit > 0 )  {
      Serial.println("Bumper hit!");
      reversing = true;
      driveTracks(3, 255, 1);
      bumperhit = 0;
      delay(1000);
      driveTracks(0, 0, 0);
      avoided = true;
    } else {
      driveTracks(3, 255, dir);
    }
  }
  reversing = false;
}

void moveOut() {
  if (drive) return;
  // reverse
  reversing = true;
  driveTracks(3, 255, 1);
  delay(750);
  reverseTurn(2);
  reversing = false;
  drive = true;
  bumperhit = 0;
}

void reverseTurn(int w) {
  // Make a reverse turn
  driveTracks(0, 0, 0);
  driveTracks(3, 255, 1);
  delay(600);
  driveTracks(w, 0, 0);
  delay(600);
  driveTracks(0, 0, 0);
}


void driveTracks(int belt, int s, bool d) {
  // d = direction
  // s = speed (0~255)
  // belt :
  // 0 = stop all
  // 1 = belt 1
  // 2 = belt 2
  // 3 = both
  if (d) {
    s = 255 - s ;
  }

  switch (belt) {
    case 0:   // stop
      digitalWrite(motorD1, 0);
      digitalWrite(motorS1, 0);
      digitalWrite(motorD2, 0);
      digitalWrite(motorS2, 0);
      break;
    case 1: // only one track
      digitalWrite(motorD1, d);
      analogWrite (motorS1, s);
      break;
    case 2: // only the other track
      digitalWrite(motorD2, d);
      analogWrite (motorS2, s);
      break;
    case 3: // both tracks
      digitalWrite(motorD1, d);
      analogWrite (motorS1, s);
      digitalWrite(motorD2, d);
      analogWrite (motorS2, s);
      break;
  }

}


/* *************************************************************************************
                 Functions for (XBEE) communication with the charger
 * *************************************************************************************
*/
bool getMessage() {
  static char *cp = bufferMessage;
  bool idMatch = false;
  if (!Serial3.available()) {
    return false;
  }

  // the first byte should match our ID
  if (Serial3.available()) {
    int thisid = (int)Serial3.read();
    if (thisid == myId) idMatch = true;
    delay(4);
  }

  while (Serial3.available()) {
    *cp = (char)Serial3.read();
    delay(4);

    if ((*cp == '\n') || (cp - bufferMessage >= 48)) // end of message character or full buffer
    {
      *cp = 0; // end of string character
      cp = bufferMessage;
      if (idMatch) strcpy(serialMessage, bufferMessage);
      return idMatch;
      break;
    }
    else {
      cp++;
    }

  }

}

void sendMessage(int id, String message) {
  Serial3.write(id);
  Serial3.println(message);
}
void clearRxBuffer() {
  char d;
  while (Serial3.available()) {
    d = (char)Serial3.read();
    delay(3);
  }
}


/* *************************************************************************************
                                     Bumpers and sensors
 * *************************************************************************************
*/

void Sensordata(int i) {
  // i=0 mid sensor only
  // i > 0 all sensors

  // First get the middle sensor
  digitalWrite(trigM, HIGH); delayMicroseconds(8); digitalWrite(trigM, LOW); delayMicroseconds(3);
  distance[1] = pulseIn(echoM, HIGH, 38000) / 58;
  delay(7);

  if (i == 0) return; // Return if i ==0

  // Now do the other 2 sensors
  digitalWrite(trigL, HIGH); delayMicroseconds(8); digitalWrite(trigL, LOW); delayMicroseconds(3);
  distance[0] = pulseIn(echoL, HIGH, 38000) / 58;
  delay(7);

  digitalWrite(trigR, HIGH); delayMicroseconds(8); digitalWrite(trigR, LOW); delayMicroseconds(3);
  distance[2] = pulseIn(echoR, HIGH, 38000) / 58;
  delay(7);
}

void bumper() {
  // This void is triggered when the bumbers are hit
  if (!reversing && !onFinalApproach) {
    if (bumperhit == 0 )driveTracks(0, 0, 0);
    if (!digitalRead(bumberL)) bumperhit = 1;
    if (!digitalRead(bumberR)) bumperhit = 2;
  }
}

/* *************************************************************************************
                                 Functions for Homing
 * *************************************************************************************
*/

boolean pingMidSensor() {
  // ping the charging station.
  // the charging station will send the signal strengths back
  clearSignalStrength();
  sendMessage(201, "P1") ;
  delay(50);

  digitalWrite(trigM, HIGH);
  delayMicroseconds(8);
  digitalWrite(trigM, LOW);
  delay(7);

  delay(200);
  if (getMessage()) {
    if (serialMessage[0] == 'L' || serialMessage[0] == 'R' || serialMessage[0] == 'E' || serialMessage[0] == 'W') {
      signalStrength[0] = (int)serialMessage[1] - 32;  // Left
      signalStrength[1] = (int)serialMessage[2] - 32;  // Right
      signalStrength[2] = (int)serialMessage[3] - 32;  // East
      signalStrength[3] = (int)serialMessage[4] - 32;  // West
    }

  } else return false;

  return true;
}

void reportSignalStrength() {
  Serial.print("LEFT="); Serial.print(signalStrength[0]);
  Serial.print(", RIGH="); Serial.print(signalStrength[1]);
  Serial.print(", EAST="); Serial.print(signalStrength[2]);
  Serial.print(", WEST="); Serial.println(signalStrength[3]);

}

void clearSignalStrength() {
  for (int c = 0; c < 5 ; c++) signalStrength[c] = 0;
}

void goHome() {
  if (homingStage == 0)findDirection(14);
  homingStage = 1;
  if (!homing) return;
  findDirection(5);
  if (!homing) return;
  pingMidSensor();
  if (chargerAdhead()) finalApproach();
  if (!homing) return;

  Sensordata(0);
  if (bestsignalstrength > 28)
    driveDistance(10, 0);
  else {
    int dis = (int)distance[1] / 2;
    dis = (dis > 60) ? 60 : dis; // maximize distance to 60cm when homing
    driveDistance(dis, 0);
  }
  if (avoided) homingStage = 0;
}


void finalApproach() {
  unsigned long t;
  Serial.println("Final Approach");
  onFinalApproach = 1;
  homing = false;
  while (digitalRead(bumberL) || digitalRead(bumberR)) {
    pingMidSensor();
    if (serialMessage[0] == 'L') crawl(1);
    if (serialMessage[0] == 'R') crawl(2);
    serialMessage[0] = '\0';

    if (!digitalRead(bumberL)) crawl(1);
    if (!digitalRead(bumberR)) crawl(2);
  }
  sendMessage(201, "H1") ;
  drive = false;
  delay(500);
  charging = true;
}

void findDirection360() {
  driveTracks(0, 0, 0);
  if (charging) return;
  int bestangle = 0;
  bestsignalstrength = 0;

  // Turn around and messure 14 angles to find the charging station.
  for (int x = 1; x < 14; x++) {
    //Serial.print("x = ");Serial.print(x);Serial.print(" ");
    pingMidSensor();
    if (chargerAdhead()) {
      finalApproach();
      return;
    }
    // we receive 4 values (L,R,E,W), find out which was the strongest

    for (int w = 0; w < 4; w++) {
      if (signalStrength[w] > bestsignalstrength) {
        bestsignalstrength = signalStrength[w];
        bestsignalsensor = w;
        bestangle = x;
        memcpy( bestsignals, signalStrength, 4 * sizeof(int) );
      }

    }
    //Serial.println();
    delay(50);
    // draai een beetje en ping nogmaals
    turnL(250);
    driveTracks(0, 0, 0);
  }

  // turn back to best angle
  if (bestangle < 6)
    for (int x = 1; x < bestangle; x++) turnL(250);
  else
    for (int x = 14; x > bestangle; x--) turnR(250);

  driveTracks(0, 0, 0);

}

void findDirection(int steps) {
  // steps = 14 or 5
  Serial.println();
  if (steps > 7) Serial.println("find direction 360");
  if (steps < 7) Serial.println("find direction 90");
  driveTracks(0, 0, 0);
  if (charging) return;
  int bestangle = 0;
  bestsignalstrength = 0;
  if (steps < 7)  turnR(500);
  // Turn around and messure 14 angles to find the charging station.
  for (int x = 1; x < steps; x++) {
    //Serial.print("x = ");Serial.print(x);Serial.print(" ");
    pingMidSensor();
    reportSignalStrength();
    if (chargerAdhead()) {
      finalApproach();
      return;
    }
    // we receive 4 values (L,R,E,W), find out which was the strongest

    for (int w = 0; w < 4; w++) {
      if (signalStrength[w] > bestsignalstrength) {
        bestsignalstrength = signalStrength[w];
        bestsignalsensor = w;
        bestangle = x;
        memcpy( bestsignals, signalStrength, 4 * sizeof(int) );
      }

    }
    // draai een beetje en ping nogmaals
    delay(50);
    turnL(250);
    driveTracks(0, 0, 0);
  }

  Serial.print("BestAngle="); Serial.println(bestangle);
  // turn back to best angle
  if (bestangle == 0) {
    Serial.print("No direction found");
    return;
  }

  if (steps > 7 && bestangle < 6) {
    int leftTurns = bestangle - 1;
    turnL(250 * leftTurns);
    Serial.print("Left turns="); Serial.println(leftTurns);
  }
  else {
    int rightTurns = (steps - 1) - bestangle;
    turnR(250 * rightTurns);
    Serial.print("Right turns="); Serial.println(rightTurns);
  }

  memcpy(signalStrength, bestsignals,  4 * sizeof(int));
  if (chargerAtEast()) {
    turnL(250);
    Serial.println("Correcting course (east)");
  }
}

void findDirection90() {
  driveTracks(0, 0, 0);
  if (charging) return;
  int bestangle = 0;
  bestsignalstrength = 0;
  turnR(500);
  // Turn around and messure only 4 angles to find the charging station.
  for (int x = 1; x < 5; x++) {
    pingMidSensor();
    if (chargerAdhead()) {
      finalApproach();
      return;
    }
    // we receive 4 values (L,R,E,W), find out which was the strongest
    for (int w = 0; w < 4; w++) {
      if (signalStrength[w] > bestsignalstrength) {
        bestsignalstrength = signalStrength[w];
        bestsignalsensor = w;
        bestangle = x;
        memcpy( bestsignals, signalStrength, 4 * sizeof(int) );
      }

    }
    delay(50);
    // draai een beetje en ping nogmaals
    turnL(250);
  }

  Serial.print("Best set: ");
  for (int e = 0; e < 4; e++) {
    Serial.print (sensors[e]); Serial.print("="); Serial.print(bestsignals[e]); Serial.print(" ");
  }
  Serial.println();

  for (int x = 4; x > bestangle; x--) turnR(250);

  driveTracks(0, 0, 0);
  memcpy(signalStrength, bestsignals,  4 * sizeof(int));
  if (chargerAtEast()) {
    turnL(250);
    Serial.println("Correcting course (east)");
  }
}


bool chargerAdhead() {
  if ((signalStrength[0] > 30 || signalStrength[1] > 30) && signalStrength[0] + signalStrength[1] > 50)  return true;
  return false;
}

bool chargerAtEast() {
  if (signalStrength[0] < 10 && signalStrength[1] < 10 && signalStrength[2] > 20) return true;
  return false;
}


void SensorTest() {

  // Test Sensors
  Serial.println("-- Start sensor test for octo pus :-)  --");

  // loop 10 times
  for (int x = 0; x < 10; x++) {
    Sensordata(1);
    Serial.print("Sensor Left = ");
    Serial.println(distance[0]);

    Serial.print("Sensor Mid = ");
    Serial.println(distance[1]);

    Serial.print("Sensor Right = ");
    Serial.println(distance[2]);
    Serial.println("-----------------------------------------");

    delay(1000);

  }
  Serial.println("--  End sensor test for octo pus :-)   --");
  

}