I uploaded the code to SourceForge so anyone can take it and do whatever they want with it and maybe in the process someone will learn something. Here it is.
Arduino autonomous tracked robot build Part 1
Arduino autonomous tracked robot build Part 2
Arduino autonomous tracked robot build Part 3
Arduino autonomous tracked robot (visual post)
I will start with the easy bits, the small functions and the bits that hold everything together.
First off, the # statements.
#include <SoftwareServo.h>
#include <IRremote.h>
#include <IRremoteInt.h>
#include <Ultrasonic.h>
#define PIN_IR 3
#define PIN_DETECT 2
#define PIN_TEMP A2
Because I had trouble with my DIY IR sensor I couldn't use the standard Servo library so I had to use the SoftwareServo library. For my sensor I used the IRRemote library and for the eyes I used the Ultrasonic library, I see now that there is a new library that could perform better but I am not sure it will work on my setup because of the whole IR thing going on.
Also I set the Infrared LED pins to digital 3, the infrared sensor to digital 2 and a temperature sensor to analog 2 just because I had one around. Using this actually helped me see if the motor shield is working with my butchered plugs.
Global variables.
Ultrasonic ultra(4,5);
IRsend irsend;
//servo sweep variables
SoftwareServo myservo; //the Servo pin 6
int CenterPos = 95; //forward centered position
int val[3]; //the 3 values in the 3 directions
int Pas=40; // used to change direction
Next I define some variables that will be used. So starting from the first, the ultra variable will be the ultrasonic sensor that is on digital pins 4 and 5. Then I declare my servo (that will be assigned the pin later) and some helper variables. The CenterPos variable holds the approximate value that will be sent to the servo for forward looking direction (a bit offset from 90 because I was not precise enough when gluing things together, but it's ok, it is easier to fix things in software than hardware). A vector to keep the values read by the ultrasonic sensor: v[0] holds what it saw to the left, v[1] what it saw in front and v[2] to the right. Pas is used for the offset from center looking position. Left position for the ultrasonic sensor is at a value of CenterPos+Pas, right is CenterPos-Pas.
//for motor shield
int dirA1 = 8;
int dirA2 = 11;
int dirB1 = 12;
int dirB2 = 13;
int speedA = 9;
int speedB = 10;
From the shield wiki page these are the variables that need to be used in order for the motors to work. The first 4 give the direction of each motor so if A1 and A2 are the same that the left motor will receive a stop command; if they are different it will receive a go clockwise or anticlockwise depending on the values (same goes for B1 and B2). The PWM signal that will be sent to the two motors is from pins 9 and 10 and using the sppedA and speedB variables.
//constants
int velocity = 70; //max speed
int turnTime = 5;// how much to turn or goBackwards
int stopDistance = 20; // safe distance
int turnOffsetVel = 25;//extract from velocity some speed when turning or going back
I defined some "constants" as a reference, a top value for the PWM cycle, velocity, when to stop if an object is too close, stopDistance, and offset velocity to subtract from top speed when turning or going back.
//variables
int spd = 0; //speed to go at
int forb = 0; //forward (1) or backward (-1)
int time = 0; // how much time is left on move
int standing = 1; //if not moving search for direction
int wentBack = 0; //previous move is backwards
//my track is not good set an offset speed for right track
int brokenTrack = 7;
And this is the end of the setup, a current speed spd, something to tell me if it is going forward or back forb, and the time left on the current move. I mentioned before that I use time (as in delay) to tell the distance it should travel and stuff like that.
Now the weird part is that my robot would turn even when going forward, the right track kept moving a bit slower than the left one. I tried to switch the 2 channels but then the left one was slower, so it is not a hardware issue. My conclusion was that either the chip in the motor shield or the motor shield itself has a small defect. My solution was, again, to fix it in software, so I always take care to put something extra when giving commands to the right track.
The functions.
First is the init function that is self explanatory. I setup some pins for output and some for input, attach the servo to pin 6, setup the irsend object and finally issue a stop command to both motors.
Now skipping to the end of the file and working my way back, there is a function there that converts the reading from the temperature sensor into degrees Celsius (because that is what makes sense to me) called GetTemp.
Then the CanBackup function that returns 1 if there aren't any obstacles in back and 0 if there are.
The GetDistance function returns the values of the ultrasonic reading and because there are some occasional problems it my return a 0 I made it return 100 in those cases since the actual value of 0 is very unlikely to happen.
So I broke down the whole code in small pieces because it is easier to maintain and understand. Following are the simple functions that allow movement.
void StopSpeed()
{
spd = 0;
forb = 0;
time = 0;
digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
standing = 1;
}
And the first one is the one that does the opposite of what I said, this is how I stop the robot. Set everything to 0 and write a digital LOW as the speed.
void TurnLeft()
{
time = turnTime;
spd = velocity-turnOffsetVel;
digitalWrite (dirA1, LOW);
digitalWrite (dirA2, HIGH);
digitalWrite (dirB1, HIGH);
digitalWrite (dirB2, LOW);
analogWrite(speedA,spd);
analogWrite(speedB,spd+brokenTrack);
standing = 0;
}
and TurnRight() functions are the same except for the direction of each track (marked by the opposite values that each corresponding variables receives A1-B1 A2-B2). Apart from that, all that is needed is to set the time for the turn and speed of the tracks (not full speed). Also note that my right track needs a bit more power then the left one.
void GoForward(int fast)
{
int offset = brokenTrack;
if (fast == 1) offset =brokenTrack * 3.7;
digitalWrite (dirA1, HIGH);
digitalWrite (dirA2, LOW);
digitalWrite (dirB1, HIGH);
digitalWrite (dirB2, LOW);
analogWrite(speedA,spd);
analogWrite(speedB,spd+offset);
standing = 0;
forb = 1;
time = -1;//continous
}
This is what it is all about. Going straight ahead(-ish). This function has a parameter, it indicates if the robot is at full speed or not. Full speed is used when there are no obstacles for miles (in this case more than 4 time the StopDistance) or at normal speed when there is something close ahead. spd, or the speed of the robot, is not set in the function, it is set outside before it was called. Now because the acceleration is not linear, the offset for my slow track needs to be adjusted, I used a factor I found close enough when testing. Also note the same values for dir*1 variables and dir*2 variables and the value given for time. -1 is used to denote continuous operation, because you always want to go forward unless you have to navigate an obstacle.
void GoBackword()
{
digitalWrite (dirA1, LOW);
digitalWrite (dirA2, HIGH);
digitalWrite (dirB1, LOW);
digitalWrite (dirB2, HIGH);
analogWrite(speedA,spd);
analogWrite(speedB,spd+brokenTrack);
standing = 0;
wentBack = 1;
time = 2*turnTime;
forb = -1;
}
This one is similar to the previous one as the spd is not given in the function, the dir*1 and dir*2 variable have same values and the time variable is given double the time of the turn mechanics.
In keeping with the idea of showing the simple stuff, there are 2 more functions that are around there.
void WaitForServo()
{
int t = 10;
while(t)
{
delay(50);
t--;
SoftwareServo::refresh();
}
}
This function is basically just a delay. Because I am using the software servo I have to give it some time for the servo to get in the desired position. While normally a simple delay would do, the fact that you can't have a large delay without affecting the position of the servo, a call to refresh() is needed every so often.
void LookAhead()
{
//look left
myservo.write(CenterPos + Pas);
//delay(500);
WaitForServo();
val[0] = GetDistance();
Serial.print(val[0]);
Serial.println(" stanga");
myservo.write(CenterPos);
//delay(500);
WaitForServo();
val[1] = GetDistance();
Serial.print(val[1]);
Serial.println(" center");
myservo.write(CenterPos - Pas);
//delay(500);
WaitForServo();
val[2] = GetDistance();
Serial.print(val[2]);
Serial.println(" dreapta");
//delay(500);
WaitForServo();
myservo.write(CenterPos);
WaitForServo();
time = -1;
}
This is used to check what is in front and to the sides. It has 3 steps that do the same thing only in different direction:
- set the desired position of the servo
- wait for it to get there
- read the distance in the appropriate variable
Now the hard functions.
I am in no way implying that this is the best approach but this is how I've done it.
void loop()
{
//return; //TODO
if(standing == 1)
{
LookAhead();
MovingForward();
}
if ( time != 0)
{
if(forb == 1)
{
MovingForward();
}
else if (forb == -1)
{
if(!CanBackup())
{
StopSpeed();
}
}
time --;
}
else
{
StopSpeed();
}
//Serial.print(GetTemp());
//Serial.println(" grade C");
delay(200);
SoftwareServo::refresh();
}
My loop function is concise and easy to understand. If you are standing look ahead and go forward. If there is a move action currently being executed (denoted by the if time!=0 statement) then if we are moving forward continue to do so if not then we must be going back; in that case check if we can continue to go back, if not we need to stop. If time has ran out we need to stop.
void MovingForward()
{
//if comming from a reverse something is blocking left right and center ahead
if(wentBack == 2)
{
ChooseLorR();
return;
}
if(val[0] > stopDistance) //left clear
{
if(val[2] > stopDistance) // right clear
{
if(val[1] >= 4*stopDistance)
{
Serial.print(spd);
Serial.println(" far ahead");
spd = velocity;
GoForward(1);
}
else if(val[1] < 4*stopDistance && val[1] > stopDistance)
{
Serial.println("near ahead");
spd = velocity /2;
GoForward(0);
}
else
{
Serial.println("stopped");
StopSpeed();
LookAhead();
ChooseLorR();
}
}
else
{
TurnLeft();
Serial.println("turning left");
}
}
else if(val[2] > stopDistance) // right clear
{
TurnRight();
Serial.println("turning right");
}
else
{
spd = velocity-turnOffsetVel;
GoBackword();
}
val[1] = GetDistance();
}
This is what you can call the heart of the brain (if that makes any sense), the main decision maker.
We start with a look to see if we just went back, that means that all ahead is blocked and we need to choose to go left or right.
If that is not the case then
- if both left and right are clear and there is nothing ahead, maximum warp;
- if left and right are clear but there is something ahead, be cautious and slowly go ahead;
- if left and right are clear than stop the collision course and look to the left or right to set a new course;
- if left is clear but right is not clear then you should go left to be sure;
- if left is not clear but right is, then right is the right path to take;
- and if all fails go back and try again (I mean the robot needs to go back, not you).
And the last function (oh god this was long)
void ChooseLorR()
{
if(val[0] > stopDistance) // left clear
{
if(val[2] > stopDistance) //right clear
{
if(val[0] > val[2])// left is more clear
{
TurnLeft();
Serial.println("turning left both clear");
}
else
{
TurnRight();
Serial.println("turning right both clear");
}
}
else
{
TurnLeft();
Serial.println("turning left no f");
}
}
else
{
if(val[2] > stopDistance) //right clear
{
TurnRight();
Serial.println("turning right no f");
}
else
{
spd = velocity-turnOffsetVel;
GoBackword();
}
}
}
As it's name implies this will try and choose a path in the right direction, even if it is the left one.
- if left and right are clear than if left is "more" clear go that way else go to the right
- if only left is clear go left
- if only right is clear go right
- and finally go back and try again if none of the above are clear
After writing everything in words I think I finally understand how my code works. I hope someone gets at least a laugh out of my attempt at an autonomous arduino tracked robot and maybe can show me the errors of my way, because this can't be the easiest way to go about this.
It's still the middle of the week but I see a Saturday on the horizon. Enjoy!
No comments:
Post a Comment