I delivered the MAEP to Nita yesterday and now she can take it to class and maybe provide a little inspiration.
The code is pretty basic at the moment, and easy to explain, so I will provide a little explanation here before I leave this project to the next generation...
In the beginning, I include the libraries for NewPing and Servo so that their code is available.
#include <Servo.h> //include Servo library
#include <NewPing.h>
The Servo library makes really easy work out of controlling a servo motor, just declare the servo output pin and then call the servo with the angle you want and the library does the rest.
The NewPing library is also there to make easy work out of the Ping or SR04 ultrasonic sensors so that you can just define a pin (or pins) for the ping and echo and then pass the results back to the program in either inches or centimeters.
Next are the definitions for the pins and variables.
#define triggerPin 10
#define echoPin 4
Servo panMotor; //Ping Pan servo
const long dangerThresh = 4.0; //threshold for obstacles (in inches)
const long longThresh = 60.0; //threshold for high speed
int leftDistance, rightDistance; //distances on either side
const int pwm = 5 ; //initializing pin 5 as pwm
const int in_1 = 6 ; //Right Motor
const int in_2 = 7 ; //Right Motor
const int in_3 = 8 ; //Left Motor
const int in_4 = 9 ; //Left Motor
These are the triggerPin, echoPin for the SR04 sensor, then the declaration of the panning servo, now created and called panMotor
There are two constans, dangerThresh which is the distance I want the robot to stop at when it sees an object. So if any object is more than 4 inches away, it will keep going...when it gets to 4 inches it stops.
The longThresh is a placeholder that could be used to control when something is really far away so that you could define different speeds based on how far away something is and then depending on the distance, set the speed of the robot. Go fast when obstacle is far away, go slow when it is close.
After than there are the pins, the pwm pin is the pin that allows you to control the speed of the motors. This is set to a Pulse Width Modulation pin on the Arduino (hence pwm).
Then you have the 4 motor pins which allow you to control the direction of each motor independently.
The next line is the creation of the sonar object for the New Ping library.
NewPing sonar(triggerPin, echoPin );
Now on to the Setup function...
void setup()
{
// initialize serial communication:
Serial.begin(9600);
//set all the pin and servo states here...
panMotor.attach(3); //attach pan motors to proper pin
panMotor.write(90); //set PING pan to center
pinMode(pwm,OUTPUT) ; //we have to set PWM pin as output
pinMode(in_1,OUTPUT) ; //Logic pins are also set as output
pinMode(in_2,OUTPUT) ;
pinMode(in_3,OUTPUT) ;
pinMode(in_4,OUTPUT) ;
}
This function initializes the Serial port so that we could watch some of the output if necessary.
Then it declares the pin the panMotor is attached to (we could have done this earlier in the constants up above as well).
Then the motor is set to 90 degrees which is the center. The reason for that is because most servos are either 180 degrees of rotation or continuous rotation. When they are 180, then 0 is pointing at one end, 90 in the middle and 180 at the other end. So in this case 90 points it roughly forward (or centered). I did not adjust this servo at all or the bracket mounted on it so the direct center is more like 80...but, nobody's perfect.
The motor pins are set as variables next so that we can use the variables in the code instead of calling each pin when the motors are used later on.
The main loop function is pretty short...but could be shorter and I will explain that more as we go on.
void loop()
{
int distanceFwd = sonar.ping_in();
Serial.print("distanceFwd is: ");
Serial.println(distanceFwd);
if (distanceFwd > dangerThresh)
{
analogWrite(pwm,150) ;
forward();
}
else //if path is blocked
{
//Brake and look both ways for distance to turn...
brake();
panMotor.write(0);
delay(1000);
//scan to the right
rightDistance = sonar.ping_in();
delay(1000);
Serial.print("rightDistance is: ");
Serial.println(rightDistance);
panMotor.write(180);
delay(1000);
//scan to the left
leftDistance = sonar.ping_in();
delay(1000);
Serial.print("leftDistance is: ");
Serial.println(leftDistance);
panMotor.write(80); //return to center (normally 90 but the servo is not centered).
delay(1000);
compareDistance();
}
}
The loop is a pretty simple one, We check the distance in front of the robot with:
int distanceFwd = sonar.ping_in();
Serial.print("distanceFwd is: ");
Serial.println(distanceFwd);
The int distanceFwd=sonar.pin_in() line says that the variable distanceFwd is an integer, and its value is equal to whatever the sonar returns for the distance to the nearest object in front of it is, in inches.
Then, we print that to the serial port so that while debugging, we can see what values are being returned.
Next there is a simple IF loop. If the distance it is seeing is greater than the dangerThresh variable (which we initialized to 4 inches at the beginning) then the car can go forward. If the distance is less than that, then the robot looks right and gets a distance to the nearest object that way, then looks left and gets the distance to the nearest object that way and then compares them to see which is greater. If it finds one greater than the other and greater than 4 inches, then it turns that way and goes back to the beginning of the loop, if it does not find one to turn to, then it turns around (180) and goes back the way it came. Then starts the loop again.
Now, you will see that the robot really does not go 'straight' or turn exactly right or left 90 degrees. That is because there are no feedback devices on the robot to tell you (or it) how far it has turned. So, to improvise, we have based the motor routines on time. If the motors had encoders or you had a compass or gyroscope chip on the robot, you could determine how far it turned or which way it was facing and then more it more precisely. Sadly, at the moment, it has none of those so to give it some basic movement, we just have the timing to use.
There are some functions below the loop to control the motors. These functions normally would be in a library (kind of like NewPing and Servo) and then just loaded into the program with an Include statement, but that really would not teach much, so they are now displayed in the main program, in the hope that as people progress in their programming skill, they will find ways to make them better and maybe create a library of their own to use for the motor controls.
The basic functions are forward(), reverse(), brake(), turnLeft(), turnRight(), quarterRight(), and quarterLeft().
These functions are pretty simple, and the basic theory is that each motor has 2 pins that have to be set to either Low or High in order to make the motor spin clockwise or counter-clockwise. This means one way is to set pin1 to low and pin2 to high and for the other way set pin1 to high and pin2 to low. This is basically like putting the positive from the battery to one side and the negative to the other. This spins the motor one way. If you reverse the pins the motor spins the other way.
now if you spin both motors clockwise, the robot moves forward. Spin them both counter-clockwise, then the robot moves backward. If you spin the right motor forward and the left motor backward, then the robot turns left. The problem here is that it will keep turning left until you stop the motor with the brake function. So, how long do you turn each motor to make a quarter of a turn (90 degrees)? Well that depends on the speed of the motors. So what we have done here is just allow them to turn and test a time delay to leave them turning and then brake after that time. That is the code you see in quarterRight() and quarterLeft().
For instance quarterRight() looks like this:
turnRight();
delay(1000);
This calls the turnRight() function and then delays the program for 1000 milliseconds (1 second) after which time it will exit that function and go to the next line of code.
So, the only other function left is the CompareDistance() function which really just uses another small IF loop to face the robot one way or the other depending on what it 'sees' for distance to the right or left. If it can turn toward either of those, it turns around and goes back the way it came.
Well, that is pretty much it for the code.
I hope someone can take this and either use it or improve upon it so that their robots will work exactly the way they want them!
Cheers,
Greg Spahn
Saturday, April 9, 2016
Wednesday, April 6, 2016
Tonight, I made the code for the MAEP and got it somewhat dialed in for its basic purpose.
I have the program set where the rover will run forward until it gets too close to an obstacle, then it will stop and look right and left and figure out which direction is better to turn, then turn and start scanning again as it is moving.
It has a few quirks...there are no encoders on the dc motors so turning is really a matter of dialing in the amount of time it takes to turn.
Also because there are no encoders, and I bridged the two pulse width pins for the motors together, there is no way to synch the two motors together so it never really travels straight.
A gyroscope chip mounted on the rover would fix both problems, but I don't have one so...it's just going to be a quirk until someone dials it in a little better.
Here is a quick video of the rover and then below is the source code I used.
Here is the source code for the project...
/*
MAEP 1.0.0 Navigation
Mobile Arduino Experimental Platform
Obstacle Avoiding Robot
GSS 4/5/2016
According to Parallax's datasheet for the PING))), there are
73.746 microseconds per inch (i.e. sound travels at 1130 feet per
second). This gives the distance travelled by the ping, outbound
and return, so we divide by 2 to get the distance of the obstacle.
See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
The speed of sound is 340 m/s or 29 microseconds per centimeter.
The ping travels out and back, so to find the distance of the
object we take half of the distance travelled.
*/
#include <Servo.h> //include Servo library
#include <NewPing.h>
#define triggerPin 10
#define echoPin 4
Servo panMotor; //Ping Pan servo
const long dangerThresh = 4.0; //threshold for obstacles (in cm)
const long longThresh = 60.0; //threshold for high speed
int leftDistance, rightDistance; //distances on either side
const int pingPin = 4; //Ping pin on Arduino
const int pwm = 5 ; //initializing pin 5 as pwm
const int in_1 = 6 ; //Right Motor
const int in_2 = 7 ; //Right Motor
const int in_3 = 8 ; //Left Motor
const int in_4 = 9 ; //Left Motor
NewPing sonar(triggerPin, echoPin );
void setup()
{
// initialize serial communication:
Serial.begin(9600);
//set all the pin and servo states here...
panMotor.attach(3); //attach pan motors to proper pin
panMotor.write(90); //set PING pan to center
pinMode(pwm,OUTPUT) ; //we have to set PWM pin as output
pinMode(in_1,OUTPUT) ; //Logic pins are also set as output
pinMode(in_2,OUTPUT) ;
pinMode(in_3,OUTPUT) ;
pinMode(in_4,OUTPUT) ;
}
void loop()
{
int distanceFwd = sonar.ping_in();
Serial.print("distanceFwd is: ");
Serial.println(distanceFwd);
if (distanceFwd > dangerThresh)
{
analogWrite(pwm,150) ;
forward();
}
else //if path is blocked
{
//Brake and look both ways for distance to turn...
brake();
panMotor.write(0);
delay(1000);
//scan to the right
rightDistance = sonar.ping_in();
delay(1000);
Serial.print("rightDistance is: ");
Serial.println(rightDistance);
panMotor.write(180);
delay(1000);
//scan to the left
leftDistance = sonar.ping_in();
delay(1000);
Serial.print("leftDistance is: ");
Serial.println(leftDistance);
panMotor.write(80); //return to center (normally 90 but the servo is not centered).
delay(1000);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance > rightDistance) //if left is less obstructed
{
quarterLeft();
brake();
delay(1000);
}
else if (rightDistance > leftDistance) //if right is less obstructed
{
quarterRight();
brake();
delay(1000);
}
else //if they are equally obstructed
{
brake();
turnAroundR();
brake();
delay(1000);
}
}
void forward ()
{
//turn left and right motors clockwise...
digitalWrite(in_1,HIGH) ;
digitalWrite(in_2,LOW) ;
digitalWrite(in_3,HIGH);
digitalWrite(in_4,LOW);
}
void reverse ()
{
//turn left and right motors counter-clockwise...
digitalWrite(in_1,LOW) ;
digitalWrite(in_2,HIGH) ;
digitalWrite(in_3,LOW) ;
digitalWrite(in_4,HIGH) ;
}
void brake ()
{
//stop left and right motors...
digitalWrite(in_1,HIGH) ;
digitalWrite(in_2,HIGH) ;
digitalWrite(in_3,HIGH) ;
digitalWrite(in_4,HIGH) ;
}
void turnRight()
{
analogWrite(pwm,150);
digitalWrite(in_1,LOW) ;
digitalWrite(in_2,HIGH) ;
digitalWrite(in_3,HIGH) ;
digitalWrite(in_4,LOW) ;
}
void turnLeft()
{
analogWrite(pwm,150);
digitalWrite(in_1,HIGH) ;
digitalWrite(in_2,LOW) ;
digitalWrite(in_3,LOW) ;
digitalWrite(in_4,HIGH) ;
}
void quarterRight()
{
//Turn about 90 degrees Right...
turnRight();
delay(1000);
}
void quarterLeft()
{
//Turn about 90 degrees Left...
turnLeft();
delay(1000);
}
void turnAroundL ()
{
//Turn all the way around to left...
turnLeft();
delay(2200);
}
void turnAroundR ()
{
//Turn all the way around to right...
turnRight();
delay(2200);
}
So, now I am hoping that someone will take it from here and improve and improvise on the idea.
I really have to go back to my Hero project so that I get that finished.
You can see that project here.
I have the program set where the rover will run forward until it gets too close to an obstacle, then it will stop and look right and left and figure out which direction is better to turn, then turn and start scanning again as it is moving.
It has a few quirks...there are no encoders on the dc motors so turning is really a matter of dialing in the amount of time it takes to turn.
Also because there are no encoders, and I bridged the two pulse width pins for the motors together, there is no way to synch the two motors together so it never really travels straight.
A gyroscope chip mounted on the rover would fix both problems, but I don't have one so...it's just going to be a quirk until someone dials it in a little better.
Here is a quick video of the rover and then below is the source code I used.
Here is the source code for the project...
/*
MAEP 1.0.0 Navigation
Mobile Arduino Experimental Platform
Obstacle Avoiding Robot
GSS 4/5/2016
According to Parallax's datasheet for the PING))), there are
73.746 microseconds per inch (i.e. sound travels at 1130 feet per
second). This gives the distance travelled by the ping, outbound
and return, so we divide by 2 to get the distance of the obstacle.
See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
The speed of sound is 340 m/s or 29 microseconds per centimeter.
The ping travels out and back, so to find the distance of the
object we take half of the distance travelled.
*/
#include <Servo.h> //include Servo library
#include <NewPing.h>
#define triggerPin 10
#define echoPin 4
Servo panMotor; //Ping Pan servo
const long dangerThresh = 4.0; //threshold for obstacles (in cm)
const long longThresh = 60.0; //threshold for high speed
int leftDistance, rightDistance; //distances on either side
const int pingPin = 4; //Ping pin on Arduino
const int pwm = 5 ; //initializing pin 5 as pwm
const int in_1 = 6 ; //Right Motor
const int in_2 = 7 ; //Right Motor
const int in_3 = 8 ; //Left Motor
const int in_4 = 9 ; //Left Motor
NewPing sonar(triggerPin, echoPin );
void setup()
{
// initialize serial communication:
Serial.begin(9600);
//set all the pin and servo states here...
panMotor.attach(3); //attach pan motors to proper pin
panMotor.write(90); //set PING pan to center
pinMode(pwm,OUTPUT) ; //we have to set PWM pin as output
pinMode(in_1,OUTPUT) ; //Logic pins are also set as output
pinMode(in_2,OUTPUT) ;
pinMode(in_3,OUTPUT) ;
pinMode(in_4,OUTPUT) ;
}
void loop()
{
int distanceFwd = sonar.ping_in();
Serial.print("distanceFwd is: ");
Serial.println(distanceFwd);
if (distanceFwd > dangerThresh)
{
analogWrite(pwm,150) ;
forward();
}
else //if path is blocked
{
//Brake and look both ways for distance to turn...
brake();
panMotor.write(0);
delay(1000);
//scan to the right
rightDistance = sonar.ping_in();
delay(1000);
Serial.print("rightDistance is: ");
Serial.println(rightDistance);
panMotor.write(180);
delay(1000);
//scan to the left
leftDistance = sonar.ping_in();
delay(1000);
Serial.print("leftDistance is: ");
Serial.println(leftDistance);
panMotor.write(80); //return to center (normally 90 but the servo is not centered).
delay(1000);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance > rightDistance) //if left is less obstructed
{
quarterLeft();
brake();
delay(1000);
}
else if (rightDistance > leftDistance) //if right is less obstructed
{
quarterRight();
brake();
delay(1000);
}
else //if they are equally obstructed
{
brake();
turnAroundR();
brake();
delay(1000);
}
}
void forward ()
{
//turn left and right motors clockwise...
digitalWrite(in_1,HIGH) ;
digitalWrite(in_2,LOW) ;
digitalWrite(in_3,HIGH);
digitalWrite(in_4,LOW);
}
void reverse ()
{
//turn left and right motors counter-clockwise...
digitalWrite(in_1,LOW) ;
digitalWrite(in_2,HIGH) ;
digitalWrite(in_3,LOW) ;
digitalWrite(in_4,HIGH) ;
}
void brake ()
{
//stop left and right motors...
digitalWrite(in_1,HIGH) ;
digitalWrite(in_2,HIGH) ;
digitalWrite(in_3,HIGH) ;
digitalWrite(in_4,HIGH) ;
}
void turnRight()
{
analogWrite(pwm,150);
digitalWrite(in_1,LOW) ;
digitalWrite(in_2,HIGH) ;
digitalWrite(in_3,HIGH) ;
digitalWrite(in_4,LOW) ;
}
void turnLeft()
{
analogWrite(pwm,150);
digitalWrite(in_1,HIGH) ;
digitalWrite(in_2,LOW) ;
digitalWrite(in_3,LOW) ;
digitalWrite(in_4,HIGH) ;
}
void quarterRight()
{
//Turn about 90 degrees Right...
turnRight();
delay(1000);
}
void quarterLeft()
{
//Turn about 90 degrees Left...
turnLeft();
delay(1000);
}
void turnAroundL ()
{
//Turn all the way around to left...
turnLeft();
delay(2200);
}
void turnAroundR ()
{
//Turn all the way around to right...
turnRight();
delay(2200);
}
So, now I am hoping that someone will take it from here and improve and improvise on the idea.
I really have to go back to my Hero project so that I get that finished.
You can see that project here.
Nita's M.A.E.P. Rover
Today I took a break from working after work, and working on my Hero Robot to work on a small project for my girlfriend and her robotics class.
Introducing MAEP Rover 1.0.0.
MAEP stands for Mobile Arduino Experimental Platform and is a term I found on the web being used by several small robotic rover types. So I decided to make one for her to use in her robotics class as another example of an experimental robot type that could be programmed fairly simply (in C) and use pretty common, off the shelf (nowadays) parts for experimentation.
I am only putting the basic control code in the 'brain' at the moment as the whole point is for people to experiment and change the rover to suit their needs, or its environment.
So, all it will do at the moment, is move until it 'sees' and obstruction, then stop, look right and left and either choose one of these two ways or turn around and try something else.
All the rest of the code will be up to her and her class to work out.
It is controlled by an Arduino Uno, a small H-bridge to control 2 small dc motors for the wheels, and a servo to swivel the Parallax Ping sensor mounted on the servo. The power is from two 3.7V rechargeable batteries providing 7.4 volts to power the whole thing. The batteries recharge pretty fast and they are readily available and provide enough power for a reasonable amount of test time between charges.
I put and off/on switch for when you are running on batteries so that it does not just 'take-off' when the power hits it, and the sensor should keep it from bashing itself into a wall...that's the theory anyway.
I will post all the basic code here and hopefully as people change it, they can update this a little further.
Keep in mind the code will be really basic as I am not sure of the class' ability to read or program in C, so instead of making libraries for the motor drivers, etc. I have just made all the features into functions that can be modified all from one program. It is not necessarily as efficient, but as their skills progress, they can make libraries and drivers on their own and learn a little along the way.
At least I hope so...
So, I will upload the code tomorrow (after I finish writing it tonight?) and then see where we go from here!
Introducing MAEP Rover 1.0.0.
MAEP stands for Mobile Arduino Experimental Platform and is a term I found on the web being used by several small robotic rover types. So I decided to make one for her to use in her robotics class as another example of an experimental robot type that could be programmed fairly simply (in C) and use pretty common, off the shelf (nowadays) parts for experimentation.
I am only putting the basic control code in the 'brain' at the moment as the whole point is for people to experiment and change the rover to suit their needs, or its environment.
So, all it will do at the moment, is move until it 'sees' and obstruction, then stop, look right and left and either choose one of these two ways or turn around and try something else.
All the rest of the code will be up to her and her class to work out.
It is controlled by an Arduino Uno, a small H-bridge to control 2 small dc motors for the wheels, and a servo to swivel the Parallax Ping sensor mounted on the servo. The power is from two 3.7V rechargeable batteries providing 7.4 volts to power the whole thing. The batteries recharge pretty fast and they are readily available and provide enough power for a reasonable amount of test time between charges.
I put and off/on switch for when you are running on batteries so that it does not just 'take-off' when the power hits it, and the sensor should keep it from bashing itself into a wall...that's the theory anyway.
I will post all the basic code here and hopefully as people change it, they can update this a little further.
Keep in mind the code will be really basic as I am not sure of the class' ability to read or program in C, so instead of making libraries for the motor drivers, etc. I have just made all the features into functions that can be modified all from one program. It is not necessarily as efficient, but as their skills progress, they can make libraries and drivers on their own and learn a little along the way.
At least I hope so...
So, I will upload the code tomorrow (after I finish writing it tonight?) and then see where we go from here!
Subscribe to:
Posts (Atom)