Go Down

Topic: The Arduino robot and freshman (Read 3070 times) previous topic - next topic

zugni

there is a code for a robot with servo and ULTRASONIC rangefinder in LM 239 .I don't like how I wrote it the fact that he turns the servo constantly and I need he for example goes found obstacle stopped turned the servo and etc. if anyone has the time please help me thank you).

Code: [Select]
#include <Ultrasonic.h>
#include <Servo.h>

#define MOTOR_1_FORWARD 8   //Вперед
#define MOTOR_1_BACK 9      //Назад
#define MOTOR_2_FORWARD 10 //Вперед
#define MOTOR_2_BACK 11    //Назад

#define TRIG 6
#define ECHO 7

#define HEAD_SERVO 5 // pIN 5

#define OBSTRACLE_DISTANCE 30.0 // РАССТОЯНИЕ ПРЕПЯТСТВИЙ
#define TURN_DELAY 200 //ОЧЕРЕДЬ ЗАДЕРЖКА 200


#define LOG false  //Журнал ложного

Ultrasonic ultrasonic(TRIG, ECHO);

Servo headServo;

int servoAngle = 90;//Угол
int angleStep = 30; //угол шага 30

float distance = 0; //Запись расстояния

void setup()
{
  pinMode(MOTOR_1_FORWARD, OUTPUT);
  pinMode(MOTOR_1_BACK,    OUTPUT);
  pinMode(MOTOR_2_FORWARD, OUTPUT);
  pinMode(MOTOR_2_BACK,    OUTPUT);
 
  stopMove(); // перестать двигаться
 
  headServo.attach(HEAD_SERVO); //руководить cервой
 
  if(LOG) Serial.begin( 9600 );//Журнал
}

void loop()
{
  updateHeadAngle();  //обновление головой угол
 
  checkDistance();    //проверить расстояние
 
  moove();            //переместите
 
  delay(200);
}

void checkDistance()    //проверьте расстояние
{
  distance = ultrasonic.Ranging(CM);   //расстояние ультразвуковом диапазоне
  if(LOG) Serial.println(distance);
}

void moove()
{
  if( distance > OBSTRACLE_DISTANCE )
  {
    if(LOG) Serial.println("FORWARD");
   
    goForward();
    delay(TURN_DELAY);
  }
  else
  {
    stopMove();
   
    checkObstracle();
  }
}

void checkObstracle()
{
  int obsLeft  = 0;
  int obsRight = 0;
 
  // График препятствий с левой и правой сторон
 
  for(servoAngle = 0; servoAngle <= 180; servoAngle += 30)
  {
    headServo.write(servoAngle);
    delay(TURN_DELAY);
   
    checkDistance();
    if(distance < OBSTRACLE_DISTANCE && servoAngle > 90) obsLeft++;
    else if(distance < OBSTRACLE_DISTANCE) obsRight++;
  }
  if(LOG) Serial.print("TURN");
   
  if(obsLeft && obsRight)
  {
    goBack();
   
    delay(TURN_DELAY * 2);
   
    if(obsLeft < obsRight) goLeft();
    else goRight();
   
    delay(TURN_DELAY);
  }
  else if(obsRight)
  {
    goLeft();
   
    delay(TURN_DELAY);
  }
  else if(obsLeft)
  {
    goRight();
   
    delay(TURN_DELAY);
  }
  else
  {
    goForward();
   
    delay(TURN_DELAY);
  }
}

void updateHeadAngle()      //обновление головой угол
{
  headServo.write(servoAngle);
 
  servoAngle += angleStep;
 
  if(servoAngle >= 150)//150
  {
    servoAngle = 150;
   
    angleStep *= -1;
  }
 
  if(servoAngle <= 30)
  {
    servoAngle = 30;   
   
    angleStep *= -1;
  }
}

void goForward()
{
  digitalWrite(MOTOR_1_FORWARD, HIGH);   //ехать вперед
  digitalWrite(MOTOR_1_BACK,    LOW);
  digitalWrite(MOTOR_2_FORWARD, HIGH);
  digitalWrite(MOTOR_2_BACK,    LOW);
}

void goBack()
{
  digitalWrite(MOTOR_1_FORWARD, LOW);
  digitalWrite(MOTOR_1_BACK,    HIGH);    //ехать назад
  digitalWrite(MOTOR_2_FORWARD, LOW);
  digitalWrite(MOTOR_2_BACK,    HIGH);
}

void goLeft()
{
  digitalWrite(MOTOR_1_FORWARD, LOW);      // поворт на лево
  digitalWrite(MOTOR_1_BACK,    HIGH);
  digitalWrite(MOTOR_2_FORWARD, HIGH);
  digitalWrite(MOTOR_2_BACK,    LOW);
}

void goRight()
{
  digitalWrite(MOTOR_1_FORWARD, HIGH);
  digitalWrite(MOTOR_1_BACK,    LOW);      // поворт на право
  digitalWrite(MOTOR_2_FORWARD, LOW);
  digitalWrite(MOTOR_2_BACK,    HIGH);
}

void stopMove()
{
  digitalWrite(MOTOR_1_FORWARD, LOW);
  digitalWrite(MOTOR_1_BACK,    LOW);     //перестать двигаться
  digitalWrite(MOTOR_2_FORWARD, LOW);
  digitalWrite(MOTOR_2_BACK,    LOW);
}

PaulS

Code: [Select]
    if(distance < OBSTRACLE_DISTANCE && servoAngle > 90) obsLeft++;
    else if(distance < OBSTRACLE_DISTANCE) obsRight++;

There is no need to compare distance to OBSTACLE_DISTANCE twice.

Code: [Select]
    if(distance < OBSTRACLE_DISTANCE)
    {
        if(servoAngle > 90)
           obsLeft++;
        else
           obsRight++;
    }

Far easier to understand, too.

We can't see what your robot is doing, or how that differs from what you want, and your description doesn't really help.
The art of getting good answers lies in asking good questions.

zugni

I'm a video don't know how to post. I will try to explain in words when he starts to go then immediately turns the servo and sometimes he does not even notice the obstacles and bumps that I don't just don't understand what's changed if you showed me the example I'm going to try and accomplish your goal again thank you for what you have responded .

zugni

that example didn't help still spinning the servo in different directions all the old

PaulS

After waving the servo around, what values are you seeing for obsLeft and obsRight?

Are you getting good readings from the ping sensor?
The art of getting good answers lies in asking good questions.

zugni

look at my code again and if not so tell me what to fix : Смайлик-путают:

zugni

Code: [Select]
#include <Ultrasonic.h>
#include <Servo.h>

#define MOTOR_1_FORWARD 8   //Вперед
#define MOTOR_1_BACK 9      //Назад
#define MOTOR_2_FORWARD 10 //Вперед
#define MOTOR_2_BACK 11    //Назад

#define TRIG 6
#define ECHO 7

#define HEAD_SERVO 5 // pIN 5

#define OBSTRACLE_DISTANCE 35.0 // РАССТОЯНИЕ ПРЕПЯТСТВИЙ
#define TURN_DELAY 200 //ОЧЕРЕДЬ ЗАДЕРЖКА 200


#define LOG false  //Журнал ложного

Ultrasonic ultrasonic(TRIG, ECHO);

Servo headServo;

int servoAngle = 90;//Угол
int angleStep = 30; //угол шага 30

float distance = 0; //Запись расстояния

void setup()
{
  pinMode(MOTOR_1_FORWARD, OUTPUT);
  pinMode(MOTOR_1_BACK,    OUTPUT);
  pinMode(MOTOR_2_FORWARD, OUTPUT);
  pinMode(MOTOR_2_BACK,    OUTPUT);
 
  stopMove(); // перестать двигаться
 
  headServo.attach(HEAD_SERVO); //руководить cервой
 
  if(LOG) Serial.begin( 9600 );//Журнал
}

void loop()
{
  updateHeadAngle();  //обновление головой угол
 
  checkDistance();    //проверить расстояние
 
  moove();            //переместите
 
  delay(200);
}

void checkDistance()    //проверьте расстояние
{
  distance = ultrasonic.Ranging(CM);   //расстояние ультразвуковом диапазоне
  if(LOG) Serial.println(distance);
}

void moove()
{
  if( distance > OBSTRACLE_DISTANCE )
  {
    if(LOG) Serial.println("FORWARD");
   
    goForward();
    delay(TURN_DELAY);
  }
  else
  {
    stopMove();
   
    checkObstracle();
  }
}

void checkObstracle()
{
  int obsLeft  = 0;
  int obsRight = 0;
 
  // График препятствий с левой и правой сторон
 
  for(servoAngle = 0; servoAngle <= 180; servoAngle += 30)
  {
    headServo.write(servoAngle);
    delay(TURN_DELAY);
   
    checkDistance();
//    if(distance < OBSTRACLE_DISTANCE && servoAngle > 90) obsLeft++;
//    else if(distance < OBSTRACLE_DISTANCE) obsRight++;
//  if(distance < OBSTRACLE_DISTANCE)
    {
        if(servoAngle > 90)
           obsLeft++;
        else
           obsRight++;
    }
 
  }
  if(LOG) Serial.print("TURN");
   
  if(obsLeft && obsRight)
  {
    goBack();
   
    delay(TURN_DELAY * 2);//2
   
    if(obsLeft < obsRight) goLeft();
    else goRight();
   
 delay(TURN_DELAY);
  }
  else if(obsRight)
  {
    goLeft();
   
    delay(TURN_DELAY);
  }
  else if(obsLeft)
  {
    goRight();
   
    delay(TURN_DELAY);
  }
  else
  {
    goForward();
   
    delay(TURN_DELAY);
  }
}

void updateHeadAngle()      //обновление головой угол
{
 headServo.write(servoAngle);
 
  servoAngle += angleStep;
 
  if(servoAngle >= 0)//150
  {
    servoAngle = 90;
   
    angleStep *= -1;
 }
// 
//  if(servoAngle <= 30)
// {
//    servoAngle = 30;   
//   
//    angleStep *= -1;
// }
}

void goForward()
{
  digitalWrite(MOTOR_1_FORWARD, HIGH);   //ехать вперед
  digitalWrite(MOTOR_1_BACK,    LOW);
  digitalWrite(MOTOR_2_FORWARD, HIGH);
  digitalWrite(MOTOR_2_BACK,    LOW);
}

void goBack()
{
  digitalWrite(MOTOR_1_FORWARD, LOW);
  digitalWrite(MOTOR_1_BACK,    HIGH);    //ехать назад
  digitalWrite(MOTOR_2_FORWARD, LOW);
  digitalWrite(MOTOR_2_BACK,    HIGH);
}

void goLeft()
{
  digitalWrite(MOTOR_1_FORWARD, LOW);      // поворт на лево
  digitalWrite(MOTOR_1_BACK,    HIGH);
  digitalWrite(MOTOR_2_FORWARD, HIGH);
  digitalWrite(MOTOR_2_BACK,    LOW);
}

void goRight()
{
  digitalWrite(MOTOR_1_FORWARD, HIGH);
  digitalWrite(MOTOR_1_BACK,    LOW);      // поворт на право
  digitalWrite(MOTOR_2_FORWARD, LOW);
  digitalWrite(MOTOR_2_BACK,    HIGH);
}

void stopMove()
{
  digitalWrite(MOTOR_1_FORWARD, LOW);
  digitalWrite(MOTOR_1_BACK,    LOW);     //перестать двигаться
  digitalWrite(MOTOR_2_FORWARD, LOW);
  digitalWrite(MOTOR_2_BACK,    LOW);
}

PaulS

Code: [Select]
  if(LOG) Serial.println(distance);
Is LOG defined? Is it true?

Code: [Select]
#define LOG false  //Журнал ложного
No, it is not true. Set it to true, so that you can see the distance value. If the sensor is not getting distance correctly, nothing you do with bogus data is going to make sense. So, the first thing we need to do is determine that the sensor IS getting distance correctly.
The art of getting good answers lies in asking good questions.

zugni

Code: [Select]
5.00
307.00
307.00
305.00
306.00
306.00
306.00
306.00
TURN307.00
FORWARD
307.00
FORWARD
307.00
FORWARD
308.00
FORWARD
309.00
FORWARD
310.00
FORWARD
310.00
FORWARD
311.00
FORWARD
310.00
FORWARD
310.00
FORWARD
309.00
FORWARD
310.00
FORWARD
309.00
FORWARD
310.00
FORWARD
309.00
FORWARD
310.00
FORWARD
309.00
FORWARD
5261.00
FORWARD
5.00
11.00
11.00
12.00
11.00
11.00
12.00
12.00
TURN306.00
FORWARD
306.00
FORWARD
307.00
FORWARD
306.00
FORWARD
306.00
FORWARD
307.00
FORWARD
308.00
FORWARD
11.00
11.00
9.00
9.00
9.00
11.00
10.00
11.00
TURN309.00
FORWARD
306.00
FORWARD
308.00
FORWARD
306.00
FORWARD
5330.00
FORWARD
5.00
305.00
305.00

here is what it seems

zugni

I like how there is one question how to add in code analogWrite so he went smoother and the fact that something started to go right here and got the idea to build traffic by using this function.

PaulS

As you can see, storing an int in a float does not improve accuracy. So, stop doing that.

Personally, I think it is pointless to print a number without some kind of ID. When you start printing obsLeft and obsRight, to make sure that stuff is being counted correctly, you'll have no way of knowing what is being printed.

Much better would be:
Code: [Select]
  if(LOG)
  {
     Serial.print("distance = ");
     Serial.println(distance);
  }

Then, you'll know what every number being printed means.

Now, if the numbers being printed seem to correlate to the actual distance to the sensor, you can comment out those 5 lines, and print obsLeft and obsRight instead.
The art of getting good answers lies in asking good questions.

zugni

Code: [Select]
102.00
distance = 102.00
FORWARD
100.00
distance = 100.00
FORWARD
98.00
distance = 98.00
FORWARD
97.00
distance = 97.00
FORWARD
94.00
distance = 94.00
FORWARD
89.00
distance = 89.00
FORWARD
83.00
distance = 83.00
FORWARD
77.00
distance = 77.00
FORWARD
70.00
distance = 70.00
FORWARD
63.00
distance = 63.00
FORWARD
56.00
distance = 56.00
FORWARD
50.00
distance = 50.00
FORWARD
45.00
distance = 45.00
FORWARD
53.00
distance = 53.00
FORWARD
70.00
distance = 70.00
FORWARD
84.00
distance = 84.00
FORWARD
94.00
distance = 94.00
FORWARD
94.00
distance = 94.00
FORWARD
103.00
distance = 103.00
FORWARD
106.00
distance = 106.00
FORWARD
110.00
distance = 110.00
FORWARD
108.00
distance = 108.00
FORWARD
108.00
distance = 108.00
FORWARD
105.00
distance = 105.00
FORWARD
106.00
distance = 106.00
FORWARD
102.00
distance = 102.00
FORWARD
106.00
distance = 106.00
FORWARD
6.00
distance = 6.00

zugni


PaulS

Quote
so it will look right?
No. Your code is still printing anonymous data.

You still have not answered the question. Does the distance data seem reasonable?

You are still using a float to store the integer distance returned by the class method.
The art of getting good answers lies in asking good questions.

zugni

Excuse me if I'm stubborn so, could You write this code for me) unless of course You have the opportunity and time:)

Go Up