Go Down

Topic: i dont get it (Read 4 times) previous topic - next topic

jada

i didnt know waht to name this post
well...
here is my problem
i wanted to do a obstacle avoiding robo a few months back and finished it (using the ping ) just simple turn away
but now i also want toi have a servo hookedup and make it look to theright and left
here is the code i am using
Code: [Select]
/////obstacle avoiding //////


#include <Servo.h>
const int pingPin = 10;            
#define ledpin_right 13                  
#define ledpin_left 12
int motor_ONE_pin_ONE=3;          
int motor_ONE_pin_TWO=4;          
int motor_TWO_pin_ONE=5;          
int motor_TWO_pin_TWO=6;
int pos = 0 ;
Servo myservo;
long dist_right =0;
long dist_left = 0;

void setup()
{
 myservo.attach(9);
 Serial.begin(9600);                    
 pinMode(ledpin_right,OUTPUT);            
 pinMode(motor_ONE_pin_ONE,OUTPUT);    
 pinMode(motor_ONE_pin_TWO,OUTPUT);    
 pinMode(motor_TWO_pin_ONE,OUTPUT);    
 pinMode(motor_TWO_pin_TWO,OUTPUT);    
}

////LOOP////

void loop ()

{
 
long duration, cm;
 pinMode(pingPin, OUTPUT);            
 digitalWrite(pingPin, LOW);          
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);          
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);          
 pinMode(pingPin, INPUT);                    
 duration = pulseIn(pingPin, HIGH);          
 cm = microsecondsToCentimeters(duration);
 Serial.print(cm);      
 Serial.print("cm");    
 Serial.println();
   
 ///servo neck centering///

 myservo.write(80);
   
if (cm > 10 )
 {
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,HIGH);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 }
 
else if (cm < 10 )
{
drive_stop();

myservo.write(10);
delay(150);
long duration, cm;
pinMode(pingPin, OUTPUT);            
digitalWrite(pingPin, LOW);          
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);          
delayMicroseconds(5);
digitalWrite(pingPin, LOW);          
pinMode(pingPin, INPUT);                    
duration = pulseIn(pingPin, HIGH);          
dist_left = microsecondsToCentimeters(duration);

Serial.print(dist_right);      
Serial.print("cm right");
pinMode(pingPin, OUTPUT);            
digitalWrite(pingPin, LOW);          
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);          
delayMicroseconds(5);
digitalWrite(pingPin, LOW);          
pinMode(pingPin, INPUT);                    
duration = pulseIn(pingPin, HIGH);          
dist_right = microsecondsToCentimeters(duration);
myservo.write(180);
delay(150);
Serial.print(dist_left);      
Serial.print("cmleft");

if(((dist_right)&&(dist_left))<(10))
   {
     digitalWrite(ledpin_left,LOW);
     digitalWrite(ledpin_right,LOW);
     drive_360();
     }
else if((dist_right)<(dist_left))
   {
     digitalWrite(ledpin_left,HIGH);
     digitalWrite(ledpin_right,LOW);
     drive_left();
   }
else ((dist_right)>(dist_left));
   {
     digitalWrite(ledpin_left,LOW);
     digitalWrite(ledpin_right,HIGH);
     drive_right();
   }
}
}


//DRIVE and other functions//


long microsecondsToCentimeters(long microseconds)
{
 return microseconds / 29 / 2;
}


void drive_forward()
{
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,HIGH);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 return;
}


void drive_stop()
{
 digitalWrite(motor_ONE_pin_ONE,LOW);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,LOW);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 return;
}

void drive_right()
{
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,LOW);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 delay(500);
 return;
}

void drive_left()
{
 digitalWrite(motor_ONE_pin_ONE,LOW);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,HIGH);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 delay(500);
 return;
 
}

void drive_360()
{
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,LOW);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 delay(500);
 return;
}


now the problem is that it until it detects a obstacle it keeps quiet i.e dosent move an inch
and even after detecting a obstacle it looks in one direction (180 degrees of servo )
and then rotates one motor only
:(
i dont know what i did wrong but i guess its the coding part

Groove

#1
Dec 01, 2010, 05:35 pm Last Edit: Dec 01, 2010, 05:36 pm by GrooveFlotilla Reason: 1
And if cm == 10 ?

Aside: why does everyone perpetuate the appallingly bad Ping driver code?
It ain't broke so don't fix it?
Per Arduino ad Astra

jada

chnaged it to cm<= 10
even now it behaves like before

cr0sh

I would suggest that to figure out your problem, you look into what a state machine is, how to make a state machine diagram, and how to apply a state machine design pattern to your system.

You'll have a few states, such as:

1) Drive in forward, while checking distance
2) Drive in reverse briefly
3) Stop
4) Scan left and right, checking distance, setting steering angle, etc

You basically set a variable you can call "state" to a value (1-n), then while in each state, you execute the commands for that state, then at the end (or when you need to change state), simply change the variable. Your states are executed by a switch-case. For instance (C-like pseudocode ahead!):

Code: [Select]
int state = 1;

loop() {
 int distance = 0;
 int angle = 0;
 switch (state) {
   case 1:
      turnWheels(0); // turn wheels to straight-ahead
      distance = driveForward(5); // move 5 units forward
      if (distance < 10) {
        state = 2;
      }
      break;
   case 2:
      turnInReverse();
      state = 3;
      break;
   case 3:
      angle = scanLeftRight(); // scan area left and right, return angle
      state = 4;
      break;
   case 4:
      turnWheels(angle); // turn wheels at angle
      distance = driveForward(5); // move forward 5 units
      if (distance < 10) {
        state = 2;
      }
      else {
        state = 1;
      }
      break;
   default:
     // hopefully we don't get here - but just in case:
     allPowerOff();
 }
}


Alright - the above is really rough, and probably won't work the way I am thinking, but it should give you an idea of how to proceed with organizing things into a coherent state machine. You could even drop the switch-case, keeping state a global variable, and inline one-after-the-other the calls to each function, and inside each function return immediately if the state doesn't match (but it won't be as easy to read - so you may want to stick to the above pattern). I am sure there are other possibilities.

Just google around for "state machine" and "state machine diagrams" to understand them more (toss in "arduino" as a search term too, of course!).

State machines are useful in a situation like this, because you have a finite number of possible "states" the machine can be in, performing an action, and you can change the state the machine is in while it is performing that action based on sensor readings (or any other criteria). It keeps things organized, and reading the flow of the code is vastly improved. It will allow you to easily see where your error lies (likely, if you code your process to a state machine pattern, you won't have the error you are running into - you may have other errors or problems, though - its not a silver bullet!)...

Hope this helps...

:)
I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

cr0sh

#4
Dec 01, 2010, 05:50 pm Last Edit: Dec 01, 2010, 05:51 pm by keeper63@cox.net Reason: 1
Quote
Aside: why does everyone perpetuate the appallingly bad Ping driver code?


Probably because doing it the "right way" using interrupts isn't very intuitive, especially for beginners...

/just a guess...

:)
I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

jada

@ cr0sh
Thanks you let me into a new method of doing things iam trying it out now !
p.s But could anyone tell me what was wrong with my code ?  :-?

jada

hi but iam having a trouble understanding the new method !  :(
so i need to provide sub-functions for everything ?
i was trying to do something and came until here
Code: [Select]
#include <Servo.h>
const int pingPin = 10;            
#define ledpin_right 13                  
#define ledpin_left 12
int motor_ONE_pin_ONE=3;          
int motor_ONE_pin_TWO=4;          
int motor_TWO_pin_ONE=5;          
int motor_TWO_pin_TWO=6;
int pos = 0 ;
Servo myservo;
long dist_right =0;
long dist_left = 0;
int state = 1;
long duration,cm;
void setup()
{
 myservo.attach(9);
 Serial.begin(9600);                    
 pinMode(ledpin_right,OUTPUT);            
 pinMode(motor_ONE_pin_ONE,OUTPUT);    
 pinMode(motor_ONE_pin_TWO,OUTPUT);    
 pinMode(motor_TWO_pin_ONE,OUTPUT);    
 pinMode(motor_TWO_pin_TWO,OUTPUT);    
}

void loop()
{
 long duration, cm;
 pinMode(pingPin, OUTPUT);            
 digitalWrite(pingPin, LOW);          
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);          
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);          
 pinMode(pingPin, INPUT);                    
 duration = pulseIn(pingPin, HIGH);          
 cm = microsecondsToCentimeters(duration);
 Serial.print(cm);      
 Serial.print("cm");    
 Serial.println();  
switch (state)
case 1:
drive_forward();
if ( cm < 10 )
{
 state = 2 ;
}
break;


case 2:
drive_stop();
state = 3;
break;


case 3:
myservo.write(10);
long duration, cm;
 pinMode(pingPin, OUTPUT);            
 digitalWrite(pingPin, LOW);          
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);          
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);          
 pinMode(pingPin, INPUT);                    
 duration = pulseIn(pingPin, HIGH);          
 dist_left = microsecondsToCentimeters(duration);
 Serial.print(dist_left);      
 Serial.print("cm");    
 Serial.println();
state = 4;
break;


case 4:
myservo.write(180);
long duration, cm;
 pinMode(pingPin, OUTPUT);            
 digitalWrite(pingPin, LOW);          
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);          
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);          
 pinMode(pingPin, INPUT);                    
 duration = pulseIn(pingPin, HIGH);          
 dist_right = microsecondsToCentimeters(duration);
 Serial.print(dist_right);      
 Serial.print("cm");    
 Serial.println();
state =5;
break;


if (dist_right < dist_left)
{
drive_left();
}
state = 6;
break;


case 6 :
if (dist_right > dist_left)
{
drive_right();
}
state = 7;
break;


case :7
if(((dist_right)&&(dist_left)) < 10 )
{
 drive_360();
}
else
{
state = 1;
}
}


long microsecondsToCentimeters(long microseconds)
{
 return microseconds / 29 / 2;
}


void drive_forward()
{
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,HIGH);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 return;
}


void drive_stop()
{
 digitalWrite(motor_ONE_pin_ONE,LOW);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,LOW);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 return;
}

void drive_right()
{
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,LOW);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 delay(500);
 return;
}

void drive_left()
{
 digitalWrite(motor_ONE_pin_ONE,LOW);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,HIGH);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 delay(500);
 return;
 
}

void drive_360()
{
 digitalWrite(motor_ONE_pin_ONE,HIGH);
 digitalWrite(motor_TWO_pin_ONE,LOW);
 digitalWrite(motor_ONE_pin_TWO,LOW);
 digitalWrite(motor_TWO_pin_TWO,LOW);
 delay(500);
 return;
}










THE CODE IS NOT CORRECT OR WORKING !!
i just posted it to ask if i was going in the right direction  :)

Groove

I'm not talking about interrupts, just factoring it into a simple function with the correct data types.
Per Arduino ad Astra

cr0sh

You're going in the right direction - however, your 7th state case check is wrong - should be "case 7:" not "case :7" (this was a typo on your part - all your other statements look OK).

I am not saying this is why it isn't working; you need to restructure your code better (notice the indentation in my example - this is VERY IMPORTANT from a code reading and maintenance standpoint), and add some comments so we know what is going on exactly.

You should probably encapsulate as much as possible into separate functions, you already have this for your motor control routines - do the same for your distance check routines, too (case/state 1, 3 & 4). You might also want to put the code for the distance check in case/state 1 inside that case statement, like you did for your left/right servo looking (3 & 4).

Your case/state 5 doesn't look right, either (missing case statement?)...

I think you are on the right track, though...

:)

I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

cr0sh

Quote
I'm not talking about interrupts, just factoring it into a simple function with the correct data types.


Well - that too; but the use of interrupts (or another method of polling without pausing), would allow you to scan and move at the same time. Most implementations of this code I have seen make the robot pause before scanning, when really the robot could be moving, scanning, pinging, etc - all in one shot. Using interrupts is one way of achieving this...
I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

Groove

Given that it takes sound 25us to travel 8mm, I'm not convinced interrupts are necessary.
Per Arduino ad Astra

bill2009

1) I'm assuming that you've verified all your subroutines so the drive and servo routines do what they are meant to.
2) The state machine seems like an excellent idea.
3) there are a few "errors" in your code that you can ignore for the moment because they won't affect your results. some others won't compile.
4) I'm pretty sure that by "if(((dist_right)&&(dist_left)) < 10 )" you mean "if( (dist_right<10) && (dist_left) < 10 ) )"
could you fix that and then put serial prints at the end of loop to tell us what state you're in and what the distances read.

I had a go below.

Code: [Select]
void loop()
{
 long duration, cm;
 pinMode(pingPin, OUTPUT);
 digitalWrite(pingPin, LOW);
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);
 pinMode(pingPin, INPUT);
 duration = pulseIn(pingPin, HIGH);
 cm = microsecondsToCentimeters(duration);
 Serial.print(cm);
 Serial.print("cm");
 Serial.println();
switch (state)
case 1:
drive_forward();
if ( cm < 10 )
{
 state = 2 ;
}
break;


case 2:
 drive_stop();
 myservo.write(10);
 pinMode(pingPin, OUTPUT);   //you should put the ping stuff in a function
 write(pingPin, LOW);
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);
 pinMode(pingPin, INPUT);
 duration = pulseIn(pingPin, HIGH);
 dist_left = microsecondsToCentimeters(duration);
 Serial.print(dist_left);
 Serial.print("cm");
 Serial.println();
 myservo.write(180);
 pinMode(pingPin, OUTPUT);
 digitalWrite(pingPin, LOW);
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);
 pinMode(pingPin, INPUT);
 duration = pulseIn(pingPin, HIGH);
 dist_right = microsecondsToCentimeters(duration);
 Serial.print(dist_right);
 Serial.print("cm");
 Serial.println();

 if (dist_right < dist_left)
 {
  drive_left();
 }
 if (dist_right > dist_left)
{
  drive_right();
 }

 if( (dist_right<10) && (dist_left) < 10 ) ){
   drive_360();
 }
 else
{
  state = 1;
 }
}



AWOL

Yup, I'm fed up with it too:
Code: [Select]
unsigned long readPing (int pingPin)
{
 pinMode(pingPin, OUTPUT);
 digitalWrite(pingPin, LOW);
 delayMicroseconds(2);
 digitalWrite(pingPin, HIGH);
 delayMicroseconds(5);
 digitalWrite(pingPin, LOW);
 pinMode(pingPin, INPUT);
 return pulseIn(pingPin, HIGH);
}


There. Done it.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

PaulS

#13
Dec 01, 2010, 10:45 pm Last Edit: Dec 01, 2010, 10:45 pm by PaulS Reason: 1
Quote
There. Done it.

Ok. Now, go fix the example page. :)

jada

Quote
I think you are on the right track, though...

feels good :) ill check it out when i come back from college though

Go Up