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);*/
}
Object avoidance, moving indoors
- Actuators / output devices: 9g servo, 2x DG02S (Dagu electronics)
- CPU: atmega328
- Power source: 5 * 1.2V rechargeable AA batteries
- Sensors / input devices: Sharp IR
- Target environment: Smooth floors
This is a companion discussion topic for the original entry at https://community.robotshop.com/robots/show/castro-iii