Castro III

Castro III

Castro III is currently a simple obstacle avoiding robot. It's built on toy car chassis.

Equipment used: Freeduino 1.22, two DC motors, Pololu DRV8833 Dual Motor Driver Carrier, 9g micro servo, Sharp IR (GP2Y0A21YK0F) 10-80cm, 5x1.2V rechargable AA batteries, 470uF cap for power, 0,1uF caps to reduce electrical noise, old led flashlight

Features:

* Runs on 5 AA recharcable battery pack

* Moves forward and tries to stay on corridor, and avoid obstacles

* Uses averages from IR readings to decide on action

Plan:

* Straight wall recognition

* Door recognition

* Some sort of crude SLAM (simultaneous localization and mapping) based on doors and walls, using only Arduino with servo and IR sensor. A robot that can recognize a wall and understand a distance between two doors would be something that I'd consider a great success.

History

1.11.2013 Learning about localization, path finding, SLAM. Planning how to make chassis sturdy enough.

12.11.2013 Better chassis, wheels glued on with superglue, axles melted to wheels with plastic.

13.11.2013 Implemented code for smooth curving movements, and decision making based on IR scan. Assembled the battery, first indoor tests without wires on. Surprisingly good on navigating on corridors, but avoiding obstacles is still poor.

16.11.2013 Assembled Castro III once more, this time with male pin headers and female connectors, and added a led flashlight in front and covered the motored wheels with old bicycle tire to get better traction. Also turned the IR sensor vertically to get better readings. Calibrated the values in the code for servo moving the IR sensor. Changes in object avoidance code. Optimized the code to make it run smoother. Result: Robot with a lot better chassis and a nice AI that succeeds in obstacle avoiding! At least most of the time. Yay! The left wheel still seems to get stuck...

https://vimeo.com/79565897

// Castro III
// by Nelsson
// This example code is in the public domain.


#include <Servo.h>
 
Servo irservo;  //Sharp IR
 
// PIN DEFINITIONS
byte irpin = 0; //Sharp IR pin
byte dca1=5; // dc motor A pin settings
byte dca2=6;
byte dcb1=3; // dc motor B pin settings
byte dcb2=11;

// OTHER
int visionfront=0; //used to interpret look()
int vision[181]; //scan() values, robot's 180 degree vision
int counter=0; //used for timing loop()
int robotx=0, roboty=0; //robot's localization X and Y on map (not yet used)
int left[20], right[20], mid[20];
byte movemode=4; //moving setting, used in motors()
byte movemodemem=0; //not yet used
byte motorpower=255; //robot's default speed
 
void setup()
{
  //Serial.begin(9600); //serial connection (debug mode)
  irservo.attach(9);  // Servo on pin 9
  pinMode(dca1, OUTPUT); //DCmotor A on pins dca1 and dca2
  pinMode(dca2, OUTPUT);
  pinMode(dcb1, OUTPUT); //DCmotor B on pins dcb1 and dcb2
  pinMode(dcb2, OUTPUT);
  pinMode(8, OUTPUT); //LED frontlight!
}

int mean(int *data, int count) //mean average function
{
    int i;
    long total;
    int result;

    total = 0;
    for(i=0; i<count; i++)
    {
        total = total + data[i];
    }
    result = total / count;
    return result;
}

void scan() //acquire new 180 deg. vision
{
  digitalWrite(8, LOW); //front led light off!!!
  int irread = 0, i=0;
  irservo.write(0);              // tell servo to go to position in variable 'pos'
  delay(500);
  /* FULL SCAN
  for(i = 0; i < 160; i += 1)  // Goes range 0-160
  {                                  // in steps of 1 degree
    irservo.write(i);              // tell servo to go to position in variable 'pos'
    delay(50);                       // waits 15ms for the servo to reach the position
    irread = analogRead(irpin);
    vision[i] = irread;
  }*/
  //OPTIMIZED SCAN
  for(i = 0; i < 20; i += 1)  // save scan() values from left
  {                                
    irservo.write(i);              // tell servo to go to position in variable 'pos'
    delay(50);                       // waits 15ms for the servo to reach the position
    irread = analogRead(irpin);
    vision[i] = irread;
    left[i] = irread;
  }
  for(i = 55; i < 75; i += 1) // save scan() values from mid
  {                                  // in steps of 1 degree
    irservo.write(i);              // tell servo to go to position in variable 'pos'
    delay(50);                       // waits 15ms for the servo to reach the position
    irread = analogRead(irpin);
    vision[i] = irread;
    mid[i-55] = irread;
  }
  for(i = 135; i < 155; i += 1) // save scan() values from right
  {                                  // in steps of 1 degree
    irservo.write(i);              // tell servo to go to position in variable 'pos'
    delay(50);                       // waits 15ms for the servo to reach the position
    irread = analogRead(irpin);
    vision[i] = irread;
    right[i-135] = irread;
  }
   irservo.write(65); //LOOK FRONT
}

int lookcorners() //acquire readings from 45 angles and reverse if blocked
{
  int leftread,rightread, i=0;
  irservo.write(30);               // move to left 45 angle
  delay(250);                       // waits 150ms for the servo to reach the position
  leftread=analogRead(irpin);        // read the Sharp IR
  delay(100);                        // delay just in case
  irservo.write(105);               // move to right 45 angle
  delay(250);                       // waits 150ms for the servo to reach the position
  rightread=analogRead(irpin);        // read the Sharp IR
  if(rightread > leftread) return rightread; else return leftread;
}


int look() //acquire Sharp IR reading from forward position
{
  int irread[5], i=0;
  irservo.write(90);              // tell servo to go front
  delay(150);                       // waits 150ms for the servo to reach the position
  for(i=0; i<5; i++)
  {
    irread[i]=analogRead(irpin);
    delay(5);
  }
  return mean(irread,5);
}

void motors() //motor commands
{
  int i;
  //movemode=4; //override for debug/programming
  if (movemode == 0)  //front
  {
    analogWrite(dca1, motorpower);
    analogWrite(dca2, 0);
    analogWrite(dcb1, motorpower);
    analogWrite(dcb2, 0);
  }
  delay(1000);
  if (movemode == 1)  //turn left
  {
    analogWrite(dca1, 0);
    analogWrite(dca2, motorpower);
    analogWrite(dcb1, motorpower);
    analogWrite(dcb2, 0);
  }
  if (movemode == 2)  //turn right
  {
    analogWrite(dca1, motorpower);
    analogWrite(dca2, 0);
    analogWrite(dcb1, 0);
    analogWrite(dcb2, motorpower);
  }
  if (movemode == 3)  //back
  {
    analogWrite(dca1, 0);
    analogWrite(dca2, motorpower);
    analogWrite(dcb1, 0);
    analogWrite(dcb2, motorpower);
  }
  if (movemode == 4)  //stop
  {
    analogWrite(dca1, 0);
    analogWrite(dca2, 0);
    analogWrite(dcb1, 0);
    analogWrite(dcb2, 0);
  }
  if (movemode == 5)  //smooth acceleration
  {
    for (i=1;i<255;i=i+10)
    {
      analogWrite(dca1, i);
      analogWrite(dca2, 0);
      analogWrite(dcb1, i);
      analogWrite(dcb2, 0);
      delay(25);
    }
    movemode=0;
  }
  if (movemode == 6)  //backwards and stop
  {
    for (i=1;i<255;i=i+10)
    {
      analogWrite(dca1, 0);
      analogWrite(dca2, i);
      analogWrite(dcb1, 0);
      analogWrite(dcb2, i);
      delay(25);
    }
    for (i=255;i>1;i=i-10)
    {
      analogWrite(dca1, 0);
      analogWrite(dca2, i);
      analogWrite(dcb1, 0);
      analogWrite(dcb2, i);
      delay(25);
    }
    movemode = 4;
  }
  if (movemode == 7)  //Curve right
  {
    analogWrite(dca1, motorpower);
    analogWrite(dca2, 0);
    analogWrite(dcb1, motorpower/3);
    analogWrite(dcb2, 0);
  }
  if (movemode == 8)  //Curve left
  {
    analogWrite(dca1, motorpower/3);
    analogWrite(dca2, 0);
    analogWrite(dcb1, motorpower);
    analogWrite(dcb2, 0);
  }
}

void think()
{
  digitalWrite(8, HIGH); //front led light on!!!
  int i, mmid,mleft,mright;
  delay(15); //delay just in case
  //decide on what to do depending on scan information
  //if(mean(mid, 20) > 120) digitalWrite(8, HIGH); //DEBUG!!
  mmid=mean(mid,20);
  mleft=mean(left,20);
  mright=mean(right,20);
  if(mleft > mright) movemode=7; //curve right
  if(mright > mleft) movemode=8; //curve left
  if(mmid < 80) movemode=0; //forward full speed
  if(mmid > 120 && mleft > mright) movemode=2;//almost coillision! Turn right!
  if(mmid > 120 && mleft < mright) movemode=1;//almost coillision! Turn left!
  if(mmid > 300) movemode=3; //REVERSE!!!
  //DEBUG!!
  /*Serial.print(mean(left,20));
  Serial.print(" ");
  Serial.print(mean(mid,20));
  Serial.print(" ");
  Serial.print(mean(right,20));
  Serial.print(" ");
  Serial.println(movemode);*/
}

void loop()
{
  counter++; //just for timing
  movemodemem=movemode; //remember old movemode (not used yet)
  movemode=4; //assign stop mode
  motors(); //stop wheels
  delay(500); //wait until there is no more movement
  scan(); //scan a 180 degree view with Sharp IR
  think(); //decide on what to do (which way to turn)
  motors(); //turn wheels
  delay(750); //wait for 0,75 seconds
  //visionfront=look(); //look forward

  /* Here is some old code
  if(visionfront > 180) {movemode=6; motors(); delay(1000);} //go reverse for one second if there is something blocking the way
  if(visionfront <= 180 && movemode==4) {movemode=5; motors(); delay(500);} //accelerate smoothly, if stopped and there is nothing on the way*/
}

  /* Interpreting look() values (Sharp IR mean average)
    60-70 (free view, almost)
    96 (2 * A4 paper)
    120 (1,5 * A4 paper)
    180 (1 * A4 paper)
    235 (0,5 * A4 paper)                                          */
    
   /* Servo angles calibrated
    myservo.write(0);              //LEFT
    myservo.write(30);              //LEFT DIAGONAL
    myservo.write(65);              //FRONT
    myservo.write(105);              //RIGHT DIAGONAL
    myservo.write(145);              //RIGHT*/

Castro II (Old version, text saved for historical purposes.)

This is my spare time project. I'm documenting this to motivate myself to work on the project. The aim is to build something small and then expand on it while the project matures. My current goals are to a) construct a reliable chassis based on babies toy car and b) code a navigation routine that is capable of solving most daily situations (getting stuck in home environment). Once the chassis works, I'm hoping to include more advanced routines (navigating to kitchen/living room, object recognition (barcode reading?)).

Cost and time to build are approximations. Cost for tools (e.g. soldering iron) is included. Time to build includes planning, problem solving and some study of electronics and programming.

Currently Castro II

* Goes backwards and forwards, turns left and right by tank steering on rear wheels

* Gets angry (variable that goes up and down) Sings happy songs randomly

* Makes annoying noises that go higher when he gets more angry Makes signal beeps

* Turns head away if annoyed, this makes his navigation quite a lot harder Turns head to decide where to go

* Suffers functional disabilities from angriness, DC motor noise and servo noise

* Sometimes drops rear wheels off

* Uses a piece of paper in front of IR sensor as a bumper signal

Equipment used: Freeduino 1.22, two DC motors, Pololu DRV8833 Dual Motor Driver Carrier, 9g micro servo, Sharp IR (GP2Y0A21YK0F) 10-80cm, 6x1.2V rechargable AA batteries, 470uF cap for power, 0,1uF caps, some small resistors, small speaker from random Chinese mobile phone

Building

For the starters, I bought a baby toy card for 0,50€ and ordered some robot parts including a Freeduino 1.22 kit, soldered the components on board with the help of great youtube tutorial from Matt Jadud (http://www.youtube.com/watch?v=BlfVFumlX3M). Assembling the rest was fairly straightforward, but as I'm a first time builder, it was tough work figuring where to put and which capacitors, and I did have some problems with Dagu's small DC motors, as they seem to be built somewhat loose and gears sometimes get into positions when they stop turning. The easiest parts were the servo and DRV8833 motor board, as the former and latter were just all about connecting them to breadboard, although later on I had to try different capacitor configurations to reduce the noise to minimum (in the end I only use 100pF in front of DC motors). I disassembled a broken Chinese mobile phone to find some small speakers, and connected one speaker through a resistor to freeduino. The final invention was to use a piece of paper (can be seen in video) in front of IR sensor as a bumper! When Castro II hits something that it wouldn't normally "see", the bumper will turn up and cover the distance sensor. Simple, but it works!

About the power supply, I read Oddbots article, and decided to go for as low voltage, but high current capacity, as possible, and chose 6 x 1.2 AA rechargables for this project. It works for the purposes of Castro II, but the batteries still don't last as long as I'd hope, the DC motors seem to draw awfully lot of current! The battery pack brought me some problems, as I bought a battery pack from local electronics store, but then found out that the anode connection was bad, and I had to solder the wires straight to the battery pack (which was hard, cause the case is plastic that melts on a low temperature). I then soldered a 470uF cap right next to battery pack and it does increase the battery time quite a bit, practically it just enables the use of DC motors.

Easily the worst part of assembly was connecting the baby toy cars wheels to small DC motors. I also don't have any precision or proper fine woodworking tools, just an axe, a big handsaw, drill, and a knife. Very crude tools! So.. The inner shaft of wheels is fairly big, so I cut a small wooden piece as a mid-piece, and then tried several configurations that did not work. The final solution was to drill a 3mm hole, about 80% through the woodpiece, then drill a smaller hole for the bolt I use for tightening the midpiece to motor. I put some hot glue to bolt to make it stick, but I have no idea if it'll last for long. Midpiece fits the toy car wheel tightly, so there's no need for glue on that part.

Programming

I have some C++ programming experience, that made me choose Arduino/Freeduino in the first place. The basic code was fairly easy, but I found out that I have to use a whole lot of delays in order to make things work properly. And as I'm used to tabletop CPU's, I also realized how slow Atmegas 328 actually is. It was an interesting process of learning to code so that it works for a robot.

The basic idea is that he goes straightforward, monitoring the distance ahead and if there is something fairly close, he reverses a bit, then looks left and right to see where is more empty space, and turns that direction. If everything is alright, he might sing a random happy song!

Video (yah, it's bad and uncut, but it exists!) https://vimeo.com/57242356

// Brains of Castro II!!!

#include <Servo.h>

//System variables
int time=0; //time since startup (ms)

//Pin settings
byte ledPin = 13;    // ledPin
byte servoPin = 2;    // servo pin
byte sensorPin = 4;    // sharp analogpin
byte dca1=4; // dc motor pin settings
byte dca2=5;
byte dcb1=7;
byte dcb2=12;

//DC motor control
int movemode=0; //front,back,right,left,stop
int moveuntilms=0; //timer to keep on doing something
int lastmovems=0; //time when a command was assigned

int sharpreading; //Sharp variable
int distanceleft,distanceright; //Sharp values in servo positions
int lastsharpmillis; //Sharp timer variable

Servo servoHead; // Servo definition
static int servoHeadD=70; // servo direction, middle=90, middle2=70

void setup() {               
  //Serial.begin(9600); //start serial connection for debug
  randomSeed(analogRead(0));
  pinMode(ledPin, OUTPUT);    
  pinMode(servoPin, OUTPUT);
  pinMode(dca1, OUTPUT); //DCmotor
  pinMode(dca2, OUTPUT); //DCmotor
  pinMode(dcb1, OUTPUT); //DCmotor
  pinMode(dcb2, OUTPUT); //DCmotor
  servoHead.attach(servoPin);
  servoHead.write(servoHeadD);
  tone(11, 1568,100); //beep
  delay(100);
  tone(11, 1319,100); //beep
  delay(150);
}

void sing() {
    int songnumber, oldmovemode;
    oldmovemode=movemode; // remember the movemode
    movemode=4;//stop for hearing a song
    movement();
    delay(100);
    songnumber=random(3);
    if(songnumber==0) {
      for (int i = 0; i < 30; i++) {   
        tone(11, 659+i*i*10,30); //beep
        delay(60-i-i);
      }
      for (int i = 0; i < 10; i++) {   
        tone(11, 3000-i*i*5,15); //beep
        delay(30);
      }
      delay(100);
      for (int i = 0; i < 10; i++) {   
        tone(11, 3000-i*i*5,15); //beep
        delay(30);
      }
    }
    if(songnumber==1) {
      for (int i = 0; i < 15; i++) {   
        tone(11, 165+i*i*i,15); //beep
        delay(30);
      }
      tone(11, 3540,150); //beep
      delay(150);
      for (int i = 0; i < 10; i++) {   
        tone(11, 3800-i*i*i,15); //beep
        delay(30);
      }
    }
    if(songnumber==2) {
      tone(11, 3520,125*(random(2)+1)); //beep
      delay(150*(random(2)+1));
      tone(11, 3520,125); //beep
      delay(125);
      tone(11, 2637,250); //beep
      delay(250);
    }
    if(songnumber==3) {
      for (int i = 0; i < 50; i++) {   
        tone(11, analogRead(sensorPin)*(analogRead(sensorPin)/100),15); //beep
        delay(30);
      }
    }
    movemode=oldmovemode;//return back to old movemode
    movement();
    delay(100);
}

void movement() {
  if(movemode==0) {
    //back
    digitalWrite(dca1, LOW);
    digitalWrite(dca2, HIGH); 
    digitalWrite(dcb1, HIGH);
    digitalWrite(dcb2, LOW);
    digitalWrite(13, HIGH); 
  }
  if(movemode==1) {
    //front
    digitalWrite(dca1, HIGH);
    digitalWrite(dca2, LOW); 
    digitalWrite(dcb1, LOW);
    digitalWrite(dcb2, HIGH);
    digitalWrite(13, LOW); 
  }
  if(movemode==2) {
    //right
    digitalWrite(dca1, HIGH);
    digitalWrite(dca2, LOW); 
    digitalWrite(dcb1, HIGH);
    digitalWrite(dcb2, LOW);
    digitalWrite(13, HIGH); 
  }
  if(movemode==3) {
    //left
    digitalWrite(dca1, LOW);
    digitalWrite(dca2, HIGH); 
    digitalWrite(dcb1, LOW);
    digitalWrite(dcb2, HIGH);
    digitalWrite(13, LOW); 
  }
  if(movemode==4) {
    //stop
    digitalWrite(dca1, LOW);
    digitalWrite(dca2, LOW); 
    digitalWrite(dcb1, LOW);
    digitalWrite(dcb2, LOW);
    digitalWrite(13, LOW); 
  }
}

void sensors() {
  lastsharpmillis=time;
  sharpreading = analogRead(sensorPin);
  delay(100);
}

void loop() {
  time=millis();
  if(time-lastsharpmillis > 250) sensors();
  //Serial.println(sharpreading);
  delay(100);
  if(sharpreading>450) {
    movemode=4; //stop for making a sound
    movement();
    delay(500);
    tone(11, 330,250); //beep
    delay(250);
    tone(11, 1319,250); //beep
    delay(250);
    tone(11, 330,250); //beep
    delay(250);
    tone(11, 1319,250); //beep
    delay(250);
    movemode=0; //backwards=0
    movement();
    delay(250); //wait for move
    sensors(); //get a new sharp reading
    delay(250);
  }
  if(sharpreading>300 && sharpreading<450) {
    //Serial.println("Action!");
    movemode=4; //stop for making a sound
    movement();
    delay(500);
    for (int i = 0; i < 8; i++) {   
      tone(11, 784,62); //beep
      delay(62);
      tone(11, 3136,62); //beep
      delay(62);
    }
    delay(1000);
    movemode=4;//stop for commands
    movement();
    delay(100);
    movemode=0;//back=0
    movement();
    delay(500);
    movemode=4;//stop for servo action
    movement();
    delay(250);
    servoHead.write(5); //turn servo left
    delay(1000); // wait for servo finish
    sensors(); //get distance to left
    delay(250);
    distanceleft=sharpreading; //the higher the closer
    servoHead.write(170); //turn servo right
    delay(1000); // wait for servo finish
    sensors(); //get distance to right
    delay(250);
    distanceright=sharpreading;
    servoHead.write(70); //turn servo to center
    delay(500); // wait for servo finish
    if(distanceright>distanceleft) {movemode=3;} else {movemode=2;} // turn left or right
    movement();
    delay(1200); //wait until turn finished
    movemode=4; //stop
    movement();
    delay(100);
  }
  if(sharpreading>150 && sharpreading<300) {
    movemode=1; //forward=1
    movement();
    delay(500);
    movemode=4; //stop=4
    movement();
    delay(50);
    sensors; //take a reading to ensure further action
    movemode=1; //forward=1
    movement();
  }
  if(sharpreading<150) {movemode=1; movement();}
  if(random(25)==5)sing(); //random sing a song
  // Dc motor test
  /*Serial.println(movemode);
  delay(100);*/
}