A little code explanation

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