Let's Make Robots!

Weird reaction of servo .

Ok everyone I had some free time and wanted to continue making my RMOAR  (you can find it with a search).

So I decided to write the code from scratch cause I had an idea about its object avoiding function.

I started writing some of the code and also debugging and testing it in every step. Allthough I figured out that when 

I added a line of code ,when the servo used for scanning was going left all were going smoothly when was going right 

suddenly the speed was increased  .So Im asking here "What Im doing wrong?" .

The code is attached to the post or can be seen below .(By the way Im using arduino for this one)

Code:

 

#include <Servo.h>

Servo s; 

int x1,x2;

int x[179];

int i = 0;

void setup(){

  pinMode(0,INPUT);

  pinMode(1,INPUT);

  pinMode(6,OUTPUT);

  Serial.begin(9600);

  s.attach(6);

}

 

void loop(){

       scan();

}

 

int readSensors() {

             x1 = analogRead(0);

             x2 = analogRead(1);

             if(x1 >= 370 || x2 >= 370) {

                 return 1;

             }

             else {

               return 0;

             }

}

int scan(){

     for(i = 0 ; i < 180; i+=1){

       s.write(i);

       delay(20);

       Serial.println(readSensors());

       x[i] = readSensors();

     }

     for( i = 180 ; i >= 1; i-=1){

       s.write(i);

       delay(20);

       Serial.println(readSensors());

     }

}

 (UPDATE: 03/01/2012):

Ok I just updated the code and fixed the array issue though another problem came up .

With the changes made what I expect it to do is to scan the area and print the position in which ,the biggest angle without any object in front of the bot ,starts .The problem Im facing is that it scans the area once and then stucks on the left side and keeps

printing the first value of z which is 0 .The code follows .

 

Code :

#include <Servo.h>
Servo s;
int x1,x2,k,z,v,a;// z is the position where the biggest angle with no obstacle in front of the bot begins. k,v,a are counters to accomplish the process of finding z .
int x[180];//An array in which is saved the state of readSensor in each position ( 0 for no obstacle , 1 for obstacle in front of the bot)
int y[180];//coutners for countring the different angle with no obstacle in front of the bot

int maxx;//max value that y[k] gets.

int i = 0;//general counter
void setup(){
  //pin assignment
  pinMode(0,INPUT);
  pinMode(1,INPUT);
  pinMode(6,OUTPUT);
  Serial.begin(9600);
  s.attach(6); // attaching the scanning servo to pin 6
}

void loop(){
       scan();

       a = 0;

       k = 0 ;

       z = 0 ;

       maxx = 0;

       for(i = 0 ; i < 180; i+=1) {

             y[i] = 0;

       }
       for(i = 0; i < 180; i+=1) {
              if(x[1] = 0) {
                k = 1;
                y[k] = 1;

               
                z = 1;
              }
              else{
                if(x[i] == 0 && x[i - 1] != 0){
                  k+=1;
                  y[k] = 1;
                }
                else if(x[i] == 0 && x[i - 1] == 0){
                 y[k] += 1;
                }
        }
        for(i = 0 ; i < k; i+=1){
          if(y[k] > maxx && y[k] != 0){
            v = 0;

            maxx = y[k];
            for(a = 0 ; a < (k - 1); a+=1){
              v = v + y[k];// sum of the previous y before y[k]
            }
            z = v + 1;
          }
        }
        Serial.println(z);
        delay(500);
       
           
    }
}
int readSensors() {
             x1 = analogRead(0);
             x2 = analogRead(1);
             if(x1 >= 370 || x2 >= 370) {
                 return 1;
             }
             else {
               return 0;
             }
}
int scan(){
     for(i = 0 ; i < 180; i+=1){
       s.write(i);
       delay(20);
      
       x[i] = readSensors();
     }
     for( i = 180 ; i >= 1; i-=1){
       s.write(i);
       delay(20);
   
      
     }
}
                 

(UPDATE: 04/01/2012): I fixed the for's but still getting the same result ,though now it prints 0 once and then prints 1 instead of 0 .

The code is above with the changes made .(maxx variable added for's fixed).

(UPDATE: 07/01/2012):Ok everyone ,I have fixed it and now it does what  I want it to do .It finds the angle in which the biggest angle without an obstacle in front of the bot starts .The code follows and I have attached the file too.

Code :

#include <Servo.h>
Servo scan_servo;
int reading1,reading2,k,turn_to;// turn_to is the position where the biggest angle with no obstacle in front of the bot begins. k is a counter to accomplish the process of finding turn_to .
int maxx;//the maximum value of angles
int state[180];//An array in which is saved the state of readSensor in each position ( 0 for no obstacle , 1 for obstacle in front of the bot)
int angles[180];//coutners for countring the different angle with no obstacle in front of the bot
int pos[180];//An array in which is saved the angle that the
int i = 0;//general counter
void setup(){
  //pin assignment
  pinMode(0,INPUT);
  pinMode(1,INPUT);
  pinMode(6,OUTPUT);
  Serial.begin(9600);
  scan_servo.attach(6); // attaching the scanning servo to pin 6
}

void loop(){
       scan();//scan the area.
       k = 0 ;
       turn_to = 0 ;
       maxx = 0;
       for(i = 0 ; i < 180; i+=1) {
          angles[i] = 0;
       }      
       for(i = 0; i < 180; i+=1) {//fill the angles array
           if(i == 1 && state[i] == 0){
             k+=1;
             angles[k] = 1;
             pos[k] = i;
                  
           }
           else if(state[i] == 0 && state[i - 1] == 0 ){
             angles[k]+=1;
           }
           else if(state[i] == 0 && state[i - 1] != 0){
             k+=1;
             angles[k] = 1;
             pos[k] = i;
           }
       }
       for(i = 0 ; i < k; i+=1){//find the biggest angles and set turn_to
         if(angles[i] > maxx){
           turn_to = pos[i] ;           
         }
       }
       Serial.println(turn_to);
       delay(500);
       
           
     }

int readSensors() {//read the sensors
             reading1 = analogRead(0);
             reading2 = analogRead(1);
             if(reading1 >= 370 || reading2 >= 370) {
               return 1;
             }
             else {
               return 0;
             }
}
int scan(){//scan the area using the scan_servo and fill the state array.
     for(i = 0 ; i < 180; i+=1){
       scan_servo.write(i);
       delay(20);
       state[i] = readSensors();
     }
     for( i = 180 ; i >= 1; i-=1){
       scan_servo.write(i);
       delay(20);
       state[i] = readSensors();
     }
    }
 (UPDATE : 08/01/2012):I think I have the whole object avoidance code ready .Testing is only what remains for it .(Propably sometime today)

Code:

#include <Servo.h>

Servo scan_servo,left_servo,right_servo; 

int reading1,reading2,k,turn_to,start_pos,state_sum;// turn_to is the position where the biggest angle with no obstacle in front of the bot begins. k is a counter to accomplish the process of finding turn_to .

int maxx;//the maximum value of angles 

long pi = 3.14 ;

int R;//radius of the wheels

int d;//distance between the wheels

int max_servo_speed = 60;//maximum value of servo's speed in rpm

int state[180];//An array in which is saved the state of readSensor in each position ( 0 for no obstacle , 1 for obstacle in front of the bot) 

int angles[180];//coutners for countring the different angle with no obstacle in front of the bot 

int pos[180];//An array in which is saved the angle that the 

int i = 0;//general counter 

void setup(){

  //pin assignment

  pinMode(0,INPUT);//set pin 0 of analog pins as analog input.

  pinMode(1,INPUT);//set pin 1 of analog pins as analog input .

  pinMode(6,OUTPUT);//set pin 6 of digital pins as digital output .

  scan_servo.attach(6); // attaching the scanning servo to pin 6 .

  left_servo.attach(7);// attaching the left continue rotation servo to pin 7.

  right_servo.attach(8);//attaching the left continue rotation servo to pin 8.

}

 

void loop(){

      if(scan() == 0){//if there is no object in front of the bot move forward till you find an object in front of the bot .

        center_scan_servo();

        do{

            move(150);

          }

        while(readSensors() == 0);

      }

      else if(scan() == 2){//if there all the area in front of bot is full of objects then turn around .

        center_scan_servo();

        turn(150,180);

      }

      else if(scan() == 1){// if you find an object in front of the bot turn a little bit to a place with no object and move forward till you find an object in front of the bot.

        center_scan_servo();

        turn(150,turn2());

        do{

            move(150);

          }

        while(readSensors() == 0);

      } 

     }

int readSensors() {//read the sensors

             reading1 = analogRead(0);

             reading2 = analogRead(1);

             if(reading1 >= 370 || reading2 >= 370) {

               return 1;

             }

             else {

               return 0;

             }

}

int scan(){//scan the area using the scan_servo and fill the state array.

     state_sum = 0;

     for(i = 0 ; i < 180; i+=1){

       scan_servo.write(i);

       delay(20);

       state[i] = readSensors();

       state_sum = state_sum + state[i];

     }

     for( i = 180 ; i >= 1; i-=1){

       scan_servo.write(i);

       delay(20);

       state[i] = readSensors();

       state_sum = state_sum + state[i];

     }

     if(state_sum == 0){

       return 0;

     }

     if(state_sum == 180){

       return 2;

     }

     else{

       return 1;

     }

    }

int turn2(){//find the angle that the robot should turn to in order not to bump into any object 

 

       k = 0 ;

       turn_to = 0 ;

       maxx = 0;

       for(i = 0 ; i < 180; i+=1) {

          angles[i] = 0;

       }       

       for(i = 0; i < 180; i+=1) {//fill the angles array

           if(i == 1 && state[i] == 0){

             k+=1;

             angles[k] = 1;

             pos[k] = i;

 

           }

           else if(state[i] == 0 && state[i - 1] == 0 ){

             angles[k]+=1;

           }

           else if(state[i] == 0 && state[i - 1] != 0){

             k+=1;

             angles[k] = 1;

             pos[k] = i;

           }

       }

       for(i = 0 ; i < k; i+=1){//find the biggest angles and set turn_to

         if(angles[i] > maxx){

           start_pos = pos[i] ;

           maxx = angles[i];           

         }

       }

       turn_to = start_pos + (maxx/2);

       if(maxx%2 != 0) {

          turn_to = turn_to + (1/2);

       }

       return turn_to;

    }

void turn(int servo_speed,int turning_angle){//the robot in the turning_angle with servo_speed 

 

        int s_speed = map(servo_speed/2,0,90,0,max_servo_speed);

        int N = (2*pi*R)/((d/2)*turning_angle);

        int t_servo = N/s_speed;

        int target_time = N * t_servo;

        left_servo.write(servo_speed);

        delay(target_time);

        right_servo.write(abs(servo_speed - 180));

        delay(target_time);

     }

 

void  move(int servo_speed) {//move the robot forward or backward depending the servo_speed

        left_servo.write(servo_speed);

        right_servo.write(servo_speed);

      }

 

void center_scan_servo(){//center the scan servo

       scan_servo.write(90);

       delay(20);

     }           

R and d variables aren't set yet cause I have to measure the radius of the wheels Im using and the distance between the wheels .

I will be uploading a video too if all go smoothly .            

AttachmentSize
object_avoiding_code_p1.zip471 bytes
object_avoiding_code_p2.zip920 bytes

Comment viewing options

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

I wrote this, it's untested though, so I'm not sure if it works. Perhaps it could give you some pointers?

 

#include <Servo.h> 

Servo myservo;  // create servo object to control a servo 
// a maximum of eight servo objects can be created 

int pos = 0;    // variable to store the servo position 

int readingsLeft[180]; // array to store readings going from right to left
int readingsRight[180]; // array to store readings going from left to right

void setup() 
{ 
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 
  Serial.begin(9600)
  } 

  int readSensors() {
    x1 = analogRead(0);
    x2 = analogRead(1);
    if(x1 >= 370 || x2 >= 370) {
      return 1;
    }
    else {
      return 0;
    }
  }

void loop() 
{ 
  for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    readingsLeft[pos] = readSensors();   // Stores the value in the array
    Serial.println(readSensors());   // Prints the value
    delay(15);                       // waits 15ms for the servo to reach the position 
  } 
  for(pos = 180; pos >= 1; pos -= 1)  // goes from 180 degrees to 0 degrees 
  {                                  // in steps of 1 degree 
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    readingsRight[pos] = readSensors();   // Stores the value in the array
    Serial.println(readSensors());   // Prints the value
    delay(15);                       // waits 15ms for the servo to reach the position 
  } 
} 

Another thing... use better variable names, you'll thank yourself later :)

Thanks for the try ,this one is more readable though I have done what you do with the code above.

The idea is that the robot will scan the area and find the direction with the bigggest angle with no object in front of it and go this way.

The problem is that Im doing something wrong on the process of finding the angle that the robot should turn .

By the way how did you managed to color the main function and comments ?Is it an automatic process or you did it manually?

These loops don't work. Variable i will always be smaller than k, because it will never be increased. Same for variable a in the inner loop.

       for(i = 0 ; i < k; k+=1){
          if(y[k] > z && y[k] != 0){
            v = 0;
            for(a = 0 ; a < (k - 1);){
              v = v + y[k];// sum of the previous y before y[k]
            }
            z = v + 1;
          }
        }
 

Just saw it , yeah Im completely dummy .  I 'll fix them and see what happens .

this is tacking some time to read this sensor the hold up might be in this  put it in the second loop as well thay should travel at the same spead and befor Serial.println(readSensors());  serial print takes it's time to  reduse your delay(20); to 10  that should even things up

 

You will need a int array with 180 elements, not 179.

int x[180];

I think 0 is an element too...

Yes the array elements are 0..179, which give you 180 in total.

This in one of the great pitfalls in C programs. You will not get a compiler warning when you access elements out of the array limits. It will just overwrite another variable.

Example1:

int var[179];   // this will allocate 179 elements of int. Valid index range is 0..178!!!

int dummy;

var[179] = 123; // this instruction will overwrite variable dummy

Example2:

int var[180];  // this will allocate 180 elements of int. Valid index range is 0..179!!!

int dummy;

var[179] = 123; // this instruction is ok

Yes ok this is what I want to do .I want every time the servo turn a little take reading see if there is an object in front of the bot

and save the state on an array .So I have to use the same i on both .

       x[i] = readSensors(); And      for(i = 0 ; i < 180; i+=1){        s.write(i); You are using that i on both