For this portion of the project, I already knew that the ping sensor was working, so I started by soldering the HC-SR04 sensor to Analog 4 & 5 on the Motor Shield as seen here:
To test my connections, I simply taped the sensor to the bumper and used the Arduino's sample ping sketch, making sure to adjust the echoPin and trigPin int's to A4 & A5 in the code. I was getting bogus values and traced the problem to faulty wire connections. I had female to female wires that were too tight to fit onto the sensors, but I carefully stretched them out and everything was good to go.
Taking small steps, I then wrote a simple code that would allow the robot to move forward until ping value became 12" or less, then back up and turn until the value was greater than 12". This was basic and allowed me to build into complexity very easily, by getting driving & turning speed as well as the ping rate sorted out until the robot was comfortably moving around on its own.
Then I mounted (hot glued) the servo to the bumper and sketched the following place holder for a 3 position scan:
#include <Servo.h>Servo myservo; // creates a servo to control, maximum 8 servos can be createdint rightpos = 45; // variable to store the right servo positionint midpos = 95; // variable to store the middle servo positionint leftpos = 145; // variable to store the left servo positionvoid setup(){myservo.attach(9); // attaches the servo on pin 9}void loop(){myservo.write(rightpos); // look rightdelay(1500); // waits 1.5 smyservo.write(midpos); // look straightdelay(1500); // waits 1.5 smyservo.write(leftpos); // look leftdelay(1500); // waits 1.5 s}
A Suppo 9g micro servo is a really good match for this ping sensor. Note that because of the mounting, a servo position of 95 was perfectly centered.
Perhaps the most important feature of the navigation code, while simple, really let the robot glide around the environment with ease and no accidentally bumping into things (unless there was a table or chair leg that the ping missed and clipped a wheel. I basically created two forward speeds so that the robot when, within a few feet of any object would slow down and have time to let the ping return info and not crash. The code is included below and is set up so that when it senses an object close, it reverses 90 degrees and begins a 3 position scan.
The next blog post will focus on expanding this code so that the robot actually does something with the 3 position scan data and chooses a smart route. Full code below:
#include <AFMotor.h>#include <Servo.h>Servo myservo;AF_DCMotor frontMotor(1); //Declaration of Front Motor for M1 of Motor ShieldAF_DCMotor rearMotor(3); //Declaration of Rear Motor for M3 of Motor ShieldAF_DCMotor steerMotor(2); //Declaration of Steer Motor for M2 of Motor Shieldconst int echoPin = A4; //sets echo to Analog pin 4const int trigPin = A5; //sets trigger to Analog pin 5int runSpeed = 255, turnSpeed = 255; //Declared speed for the Motorint rightpos = 45; // variable to store the right servo look positionint midpos = 95; // variable to store the middle servo look positionint leftpos = 145; // variable to store the left servo look positionvoid setup() {// initialize serial communication:Serial.begin(9600);myservo.attach(9); // attaches the servo on pin 9 to the servo objectfrontMotor.setSpeed(runSpeed);rearMotor.setSpeed(runSpeed);steerMotor.setSpeed(turnSpeed);frontMotor.run(RELEASE);rearMotor.run(RELEASE);steerMotor.run(RELEASE);}void loop(){// establish variables for duration of the ping,// and the distance result in inches and centimeters:long duration, inches, cm;// The sensor is triggered by a HIGH pulse of 10 or more microseconds.// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:pinMode(trigPin, OUTPUT);digitalWrite(trigPin, LOW);delayMicroseconds(2);digitalWrite(trigPin, HIGH);delayMicroseconds(10);digitalWrite(trigPin, LOW);// Read the signal from the sensor: a HIGH pulse whose// duration is the time (in microseconds) from the sending// of the ping to the reception of its echo off of an object.pinMode(echoPin, INPUT);duration = pulseIn(echoPin, HIGH);// convert the time into a distanceinches = microsecondsToInches(duration);Serial.print(inches);Serial.print("in, ");Serial.println();delay(100);if (inches <= 36 && inches >= 13){AllForwardSlow();}else if (inches <= 12){backupTurn();lookAround();}else{AllForward();}}long microsecondsToInches(long microseconds){// 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.return microseconds / 74 / 2;}void AllStop(){Serial.println("Stopping...");frontMotor.run(RELEASE); // stops front motorrearMotor.run(RELEASE); // stops rear motorsteerMotor.run(RELEASE); // stops steering motor}void AllForwardSlow(){Serial.println("Going forward Slowly...");(runSpeed = 100);steerMotor.run(RELEASE); // stops steering motorfrontMotor.run(FORWARD);frontMotor.setSpeed(runSpeed);rearMotor.run(FORWARD);rearMotor.setSpeed(runSpeed);}void AllForward(){Serial.println("Going forward...");(runSpeed = 150);steerMotor.run(RELEASE); // stops steering motorfrontMotor.run(FORWARD);frontMotor.setSpeed(runSpeed);rearMotor.run(FORWARD);rearMotor.setSpeed(runSpeed);}void backupTurn(){Serial.println("backing up and turning...");(runSpeed = 255);steerMotor.run(FORWARD);steerMotor.setSpeed(turnSpeed);frontMotor.run(BACKWARD);frontMotor.setSpeed(runSpeed);rearMotor.run(BACKWARD);rearMotor.setSpeed(runSpeed);delay (1500);}void lookAround(){Serial.println("Stopping to Look...");frontMotor.run(RELEASE); // stops front motorrearMotor.run(RELEASE); // stops rear motorsteerMotor.run(RELEASE); // stops steering motordelay (1500);Serial.println("Looking Around...");myservo.write(rightpos); // tell servo to go to right positiondelay(1500);myservo.write(midpos); // tell servo to go to middle positiondelay(1500); // waits 1.5 seconds for the servo to reach the position & scanmyservo.write(leftpos); // tell servo to go to left positiondelay(1500);myservo.write(midpos); // re-center servo to go to middle position}
End Phase 4.