MyTurtle: self-learning robot (Part 3 - Head and Bumpers)

In the previous part (Part 2 - Movement unit) I wrote about making a chassis for the movement unit of the self-learning robot MyTurtle. In this part I am going to add checking obstacles with the ultrasonic sensor HC-SR04, bumpers to record a hit and show all the actual code to serve the movement unit.

This project has a longer code so I created more tabs in the software to make things clear and easily accessible. Parts of the code lower are from these tabs.

Ultrasonic head

As I wrote in the first part (Part 1 - Basic idea) MyTurtle will think and learn. These two basic assumptions forced me to decide how it will work with the ultrasonic sensor to avoid obstacles. As I mentioned I do not want to tell MyTurtle what to do. It can only check the actual status (Neuron) and react to it (Action) due to its previous experiences.

So I decided that MyTurtle will always see the obstacle in front of its head. But it can turn its head to the left, to the right or forward. Moreover it can check obstacles around it. In this case it will not see an obstacle in front of its head only but all around (left, front, right).

The code about the ultrasonic sensor contains 2 functions - isObstacle() and CheckObstacles(). The return value of isObstace() is true if an obstacle is 10cm or less in front of the head. CheckObstacles() returns the status of obstacles around to write into Neuron. We will talk about it in other parts.

#define echoPin 12
#define trigPin 13
#define setTrigPinHigh (PORTB |= B00100000)
#define setTrigPinLow (PORTB &= B11011111)
const int safeDistance = 10;
void InitializeUltrasonic() {
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);
}
bool isObstacle() {  //checks if there is an obstacle X cm far or less
 int distance;
 setTrigPinLow; 
 delayMicroseconds(2);
 setTrigPinHigh; 
 delayMicroseconds(10);
 setTrigPinLow; 
 distance = pulseIn(echoPin, HIGH) / 58.2;
 return (distance < safeDistance);
}
byte CheckObstacles() {
 byte x = 0;
 LookLeft();
 if (isObstacle()) x |= B00000100;
 LookRight();
 if (isObstacle()) x |= B00000001;
 LookForward();
 if (isObstacle()) x |= B00000010;
 return x;   // =B00000LFR
}

The part about the servo that turns head is below. I had to put delay(500) in functions LookLeft(), LookRight() and LookFront() to wait for the servo motor to do its work:

#define servoPin 3
const int leftAngle = 150;
const int rightAngle = 30;
const int frontAngle = 90;
Servo Head;
void InitializeHead() {
 pinMode(servoPin, HIGH);
 Head.attach(servoPin); 
}
void TurnHeadForward() {
 int k = Head.read();
 if (k < frontAngle) {
   for (k; k <= frontAngle; k++) {
     Head.write(k);   //set angle of servo motor  
     delay(20);
   }
 } else {
     for (k; k >= frontAngle; k--) {
     Head.write(k);   //set angle of servo motor  
     delay(20);
   }
 }
}
void TurnHeadLeft() {
 for (int k = Head.read(); k <= leftAngle; k++) {
   Head.write(k);   //set angle of servo motor  
   delay(20);
 }
}
void TurnHeadRight() {
 for (int k = Head.read(); k >= rightAngle; k--) {
   Head.write(k);   //set angle of servo motor
   delay(20);
 }
}
void LookLeft() {
 Head.write(leftAngle);
 delay(500);
}
void LookRight() {
 Head.write(rightAngle);
 delay(500);
}
void LookForward() {
 Head.write(frontAngle);
 delay(500);
}
byte GetHeadStatus() {
 int k = Head.read();
 if (k > frontAngle + 5) return B00000010; //left
 if (k < frontAngle - 5) return B00000001;  //right
 return B00000000;  //front
}

Bumpers

It was a technical challenge for me to create bumpers. Actually I made the front one. MyTurtle will have a fear to hit something so I needed a bumper to record a hit of MyTurtle. Finally I made the bumper from a piece of wood and 2 nails. Nails are attached to the bumper and freely move inside of MyTurtle so they can push the left or right switch as in the pictures:

The circuit of the switch is as following. Switches are attached to pins A0, A1, A2 and A3. Their default is "switched on", so they set LOW to pin A0, A1, A2 or A3. If the bumper is pushed, the state of a button is "switched off" and it sets the pin HIGH. I used 2700 ohm resistor but you can use any from 200 to 2700 ohms.

The code for bumpers is following. I used the quick port reading that Arduino allows because I do not want motors to move if there is an obstacle (to protect them) and we will need to use these functions in bigger cycles.

void InitializeBumpers() {
 DDRC &= B11110000;   //set pin A0-A3 as INPUT
}
bool FrontBumpersPushed() {
 if (((PINC & B01) == B01) || ((PINC & B10) == B10)) return true;   //digitalRead(A0) || digitalRead(A1)
 return false;
}
bool LeftFrontBumperPushed() {
 if ((PINC & B01) == B01) return true;   //digitalRead(A0);
 return false;
}
bool RightFrontBumperPushed() {
 if ((PINC & B10) == B10) return true;   //digitalRead(A1)
 return false;
}
bool RearBumpersPushed() {
 //if (((PINC & B0100) == B0100) || ((PINC & B1000) == B1000))) return true;          
 return false;
}

And here is the whole code for step motors. I plan to add detections of the floor under MyTurtle to avoid falling from the table so the "interruption" will be used by them to stop motors.

As you can see from the code below the bumpers do not allow motors to move, but the interruption stops them once only. It allows them to move in the next step. If there is an edge of the table under MyTurtle, moving process must stop and allow MyTurtle to evaluate what happens. But in the next step it is up to MyTurtle if it decides to move in the same direction and to fall or change the direction. If it falls of the table the gyroscope sensor will tell that an inclination is more than 15 degrees and MyTurtle knows it is bad for it. So next time it will try to avoid this bad experience (it will be solved in the part about its brain).

#define CheckFrontBumpers ({ if (FrontBumpersPushed() || interruption) { interruption = false; return; } })
#define CheckRearBumpers ({ if (RearBumpersPushed() || interruption) { interruption = false; return; } })
#define CheckLeftFrontBumper ({ if (LeftFrontBumperPushed() || interruption) { interruption = false; return; } })
#define CheckRightFrontBumper ({ if (RightFrontBumperPushed() || interruption) { interruption = false; return; } })
#define CheckInterruption ({ if (interruption) { interruption = false; return; } })
const int stepsPerRevolution = 2048; 
const int rpm = 15;   //set speed of motors (max 16 for these step motors)
Stepper LeftMotor = Stepper(stepsPerRevolution, 8, 10, 9, 11);
Stepper RightMotor = Stepper(stepsPerRevolution, 4, 6, 5, 7);
void InitializeMotors() {
 LeftMotor.setSpeed(rpm);
 RightMotor.setSpeed(rpm);
}
void MoveForward(int cycle) {
  for (int k=0; k < cycle; k++) {
    CheckFrontBumpers;
    LeftMotor.step(1);
    RightMotor.step(1);
  }
}
void MoveBack(int cycle) {
  for (int k=0; k < cycle; k++) {
    CheckRearBumpers;
    LeftMotor.step(-1);
    RightMotor.step(-1);
  }
}
void MoveLeftForward(int cycle) { 
  for (int k=0; k < cycle; k++) {
    CheckFrontBumpers;
    RightMotor.step(1);
  }
}
void MoveRightForward(int cycle) {  
  for (int k=0; k < cycle; k++) {
    CheckFrontBumpers;
    LeftMotor.step(1);
  }
}
void MoveLeftBack(int cycle) {  
  for (int k=0; k < cycle; k++) {
    CheckRearBumpers;
    RightMotor.step(-1);
  }
}
void MoveRightBack(int cycle) {  
  for (int k=0; k < cycle; k++) {
    CheckRearBumpers;
    LeftMotor.step(-1);
  }
}
void TurnLeft(int cycle) {    
  for (int k=0; k < cycle; k++) {
    CheckRightFrontBumper;
    LeftMotor.step(-1);
    RightMotor.step(1);
 }
}
void TurnRight(int cycle) { 
  for (int k=0; k < cycle; k++) {
    CheckLeftFrontBumper;
    LeftMotor.step(1);
    RightMotor.step(-1);
  }
}

The interruption part:

#define interruptPin 2
void InitializeFloor() {
 pinMode(interruptPin, LOW);
 attachInterrupt(0, pin_ISR, CHANGE);
}
void pin_ISR() {
 interruption = ((PORTD & B00000100) == B00000100);  //digitalRead(2);
}

The main part of the code

This part of the code puts it all together. MyTurtle does not have a code for self-learning process yet but we can already use it as a car robot. So I set 4 modes that MyTurtle can be in:

• Random mode does randomly all the actions MyTurtle can do

• Manual mode receives orders from an Android app that we will discuss about in the next part

• Program mode receives a chain of orders from the Android app

• Self-learning mode will let everything on MyTurtle - on its experiences and learning

/* Basic robot car as a preparation for creating a self-learning robot
* by Viliam Krunka
*/
#include <Stepper.h>
#include <Servo.h>
char BTData;
char mode = 0;
const byte actionNum = 13;
bool interruption = false;
void setup() {
 InitializeBlueTooth();
 InitializeHead();
 InitializeMotors();
 InitializeUltrasonic();
 InitializeBumpers();
 InitializeFloor();
}
void loop() {
 switch (mode) {
   case 0:   //random
     DoAction((char) random(actionNum), true);
     getBTDataLast();
     break;
   case 1:   //manual
     DoAction(BTData, false);
     getBTDataLast();
     break;
   case 2:   //program
     DoAction(BTData, true);
     getBTDataEvery();
     break;
   case 3:   //self-learning
     getBTDataLast();
     break;      
 }
}
void DoAction(char a, bool cycle) {
 switch(a) {
   case 0:
     delay(3000);      //DoNothing();
     break;
   case 1:
     MoveForward(cycle? 670:1);  //4cm forward
     break;
   case 2:
     MoveBack(cycle? 670:1);   //4cm back
     break;
   case 3:
     MoveLeftForward(cycle? 1650:1);  //45 degrees
     break;
   case 4:
     MoveRightForward(cycle? 1650:1);  //45 degrees
     break;
   case 5:
     MoveLeftBack(cycle? 1650:1);  //45 degrees
     break;
   case 6:
     MoveRightBack(cycle? 1650:1);  //45 degrees
     break;
   case 7:
     TurnLeft(cycle? 800:1);   //45 degrees
     break;
   case 8:
     TurnRight(cycle? 800:1);  //45 degrees
     break;
   case 9:
     TurnHeadForward();
     break;
   case 10:
     TurnHeadLeft();
     break;
   case 11:
     TurnHeadRight();
     break;
   case 12:
     CheckObstacles();
     break;
 }
} 

and the bluetooth part:

void InitializeBlueTooth() {
 Serial.begin(9600);
}
void getBTDataLast() {
 while (Serial.available()) {  //get the last data
   BTData = Serial.read();
   setMode();
 }
}
void getBTDataEvery() {
 if (Serial.available()) {   
   BTData = Serial.read();
   setMode();
   if (BTData >= '1' && BTData <= '9') BTData -=48;
 } else {
   BTData = 200;
 }
}
void setMode() {
 if (BTData >= 100) {
   mode = BTData - 100;
 }
}

I am going to write about the bluetooth module and the Android app in the next part.

Actual links

Part 1 - Basic idea

Part 2 - Movement unit

Part 4 - Android app