Let's Make Robots!

Arduino Programming problem from a beginner

I am working on my LMR First robot. It is arduino equiped with a motor shield. Two motors, a servo and a IR sensor for object avoidance.

 

Here is my code#include <Servo.h>
#include <AFMotor.h>
#include <NewPing.h>


AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
AF_DCMotor motor1(1, MOTOR12_64KHZ);

#define TRIGGER_PIN  15
#define ECHO_PIN     14
#define MAX_DISTANCE 200

int servoPin = 10;
int servoRange[] = {500,2500};
int servoRotateTime = 500;

int dangerDist = 1000;
int servoTime = 700;
int turnTime = 250;
int pos = 0;


NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

Servo servo;



void setup() {
  Serial.begin(115200);           // set up Serial library at 9600 bps
  Serial.println("C'est Partit!");
 
 
  motor2.setSpeed(110);     // set the speed to 200/255
  motor1.setSpeed(125);
 
  servo.attach(10);
  servo.write(90);
  delay(50);
}
void loop (){
 unsigned int uS = sonar.ping();
  Serial.print("Ping: ");
  Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
  Serial.println("cm");
  
  
   if(uS > dangerDist){
    Serial.println("All good");
    motor1.run (FORWARD);
    motor2.run(FORWARD);
    delay(50);
   }
   

  else //if path is blocked
  {
    motor1.run (RELEASE);
    motor2.run (RELEASE);
    servo.write(180);
    delay(500);
    unsigned int left = sonar.ping();
    servo.write(10);
    delay(500);
    unsigned int right = sonar.ping();
    servo.write(90);
  }
    if(left < dangerDist){
     motor1.run(FORWARD);
    delay(turnTime);
    }
    if(left > dangerDist){
      motor2.run(FORWARD);
      delay(turnTime);
    }
}
   
  
 
  I would like to have my IR sensor turning left and right with my servo collecting data from what he saw left and right and judge if its over the danger dist (established at 10cm). The problem is I cant get the variable ''right'' or ''left'' to be attached to the ping value the IR saw on either left or right side of the robot.

 

Thank you,

Remember Im an absolute beginner

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

In the loop() function you have the line 

if( currDist < dangerlevel ) {   nodanger();} else ...

That implies it should run the nodanger() function ONLY if an obstacle is CLOSER than the dangerlevel.  I think if you place an obstacle in front of the sonar it will drive right into it.  To fix it, change the currDist < dangerlevel to currDist > dangerlevel.

As for the if(DEBUG) it was apparently put there to help debug the program.  It is turned off.  The #define DEBUG 0  on the fourth line sets DEBUG to 0 so the if(DEBUG) doesn't execute.  By changing to #define DEBUG 1 it will cause those lines to execute every time through the loop.

The exact model for my ultrasonic ranging sensor is HC-SR04

Im not too sure how to declare those two value since they will be fluctuating all the time. They will vary from 0 to the maximum range  value. I will try to read on that segment of my code.

 

For the delay part in my void setup I added that part to have time to place the power then place the shell of my robot over my electronic. I dont have a on off switch so its kind of sketchy for now. I should add a photo to it to make it clearer for you guys.

 

Thanks for all the good advices!

I have modify my code to make it look like the robot ''Penny'' from ignoblegnome. It is adapted to my robot. Everything seems to function except the nodanger void or all forward. The ping is collected and the value is greater then the dangerlevel value but it seems to stay in place and scan left and right for ever. So I might need your help again on this one guys. There is also the debug fonction in the loop and I cant understand the purpose of it.

 

So here is my actual code:

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define DEBUG 0


AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
AF_DCMotor motor1(1, MOTOR12_64KHZ);
 
#define TRIGGER_PIN  15
#define ECHO_PIN     14


NewPing sonar(TRIGGER_PIN, ECHO_PIN);
Servo headservo;  // creates servo object
int dangerlevel = 600;   // how far away should things be, before we react?
int turn = 300;          // this sets how much we should turn
int servo_turn = 700;    //

int currDist = 0;    // holds the current distance
int compDist = 0;  // holds a distance to compare currDist to

void setup(){
  Serial.begin(9600);
  Serial.println("C'est Partit");
  motor2.setSpeed(110);     // set the speed to 200/255
  motor1.setSpeed(125);
  headservo.attach(10);
  headservo.write(70);
  delay(15000);
}
void loop() {
  currDist = sonar.ping();
  Serial.print("Ping: ");
  Serial.print(currDist / US_ROUNDTRIP_CM);
 Serial.println("cm");
  if(currDist < dangerlevel) {
    nodanger();      // if no danger, drive ahead
    }
  else {
    whichway();      // if obstacle ahead then decide with way is better
  }
//  delay(500);
  if(DEBUG){
    Serial.print("Current Distance: ");
    Serial.println(currDist);
  }
}

void nodanger() {
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  Serial.println("Enwouaye en avan" );
  return;
}
void whichway() {
  totalhalt();
 
  lturn();
  delay(servo_turn);
  int lDist = sonar.ping();
  totalhalt();
 
  rturn();
  delay(servo_turn);
  int rDist = sonar.ping();
  totalhalt();
 
  if(lDist < rDist) {
    body_lturn();
  }
  else {
    body_rturn();
  }
  return;
}

void totalhalt() {
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  headservo.write(70);
  delay(1000);
}
void lturn() {
  headservo.write(120);
}
void rturn(){
  headservo.write(20);
}

void body_rturn() {
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
  delay(turn);
  totalhalt();
}
void body_lturn() {
  motor1.run(BACKWARD);
  motor2.run(FORWARD);
  delay(turn);
  totalhalt();
}

 

Please note that since my first language is french I have added some french comments in the serial.println to visual the code while its running.

 

Thanks

You have to make sure that you use the right sensor with the right code. Ping is for ultrasound, like the SF-04 and analog read for IR sensors like the Sharp GD**** The below code is a snipped from my Chopstick Junior and using the Sharp IR sensor.

void sweep()
{  
    for(pos = 20; pos < 160; pos += 1)   // goes from 20 degrees to 160 degrees 
    {                                    // in steps of 1 degree 
      eye.write(pos);                    // tell servo to go to position in variable 'pos' 
      delay(10);                         // waits 10ms for the servo to reach the position 
      distance = analogRead(sensor);
      //Serial.print(distance);
      //Serial.print(" - Position ");
      //Serial.print(pos);
      //Serial.println();
    } 
    for(pos = 160; pos >= 20; pos -= 1)      // goes from 160 degrees to 20 degrees 
    {                                
      eye.write(pos);                    // tell servo to go to position in variable 'pos' 
      delay(10);                         // waits 10ms for the servo to reach the position 
      distance = analogRead(sensor);
      //Serial.print(distance);
      //Serial.print(" - Position ");
      //Serial.print(pos);  
      //Serial.println();
    }       
}  

You ask about IR and then include the NewPing lib. Which sensor type are you using?

Also, there are two points in your code that you define/declare variables, and then, don't utilize them.
int servoPin = 10;
servo.attach(10);

int servoRotateTime = 500;
delay(500);

I agree with Max. You should probably declare your left and right variables above, and then, fill them as needed in the main loop.

It is an arduino duel... And you are also correct for the robot turning it would make more sens for him to be rotating on his axis.

 

Try declaring "left" and "right" in the setup and assigning the values in the loop instead of using the assignation as the declaration. Also, which Arduino is this?  On an Uno footprint your pins are one thing and on the Due footprint they're quite another.

Is it your intention that the robot actually turn forward rather than on its axis?