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.

No comments:

Post a Comment