Pages: [1]   Go Down
Author Topic: Roboter mit Ultraschallsensor  (Read 1039 times)
0 Members and 1 Guest are viewing this topic.
Austria
Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hallo!
Ich ein Problem mit meinem Roboter. Wenn ich ihn einschalte dreht sich immer nur das linke Rad. Wenn ich aber ein anderes Programm auf den Arduino hochlade, fährt der Roboter ganz normal. Zum Ansteuern der beiden Motoren verwende ich folgenden Motortreiber.
http://www.physicalcomputing.at/shop/article_A-1123300/DC-Motor-Treiber-1A%2C-Dual.html?sessid=Fahcu3BeyllTzv0Cg6LmU0BpKMjDXyjAPTFvvHD7eV2HFJy3QSmhQOi5aSriuIl8&shop_param=cid%3D23%26aid%3DA-1123300%26
Bitte um Hilfe!
Roboter-Programm
Code:
#include <Servo.h>
#include <Ping.h>
#include <Motor.h>

const int PWMA_rechts=10;  //rechter Motor
const int PWMB_links=11;   //linker Motor
const int AIN1=2, AIN2=3, BIN1=4, BIN2=5; //AIN1,AIN2,BIN1 und BIN2 sind die steuereingänge des Motortreibers ( ob er Vorwährts oder Rückwährts fahren soll)
const int stby=6, pingPin=8, speedPin=0, servoPin=9;
const int mitte=89, links=0, rechts=179;
int roboSpeed, value;
long abstand, abstandLinks=1000, abstandRechts=1000;
Motor motor(PWMA_rechts,PWMB_links,AIN1,AIN2,BIN1,BIN2); //Motorinitialisierung
Ping sensor(pingPin);//Ultraschallsensorinitialisierung
Servo servo;

void setup()
{
  Serial.begin(9600);
  pinMode(stby,OUTPUT);
  motor.Start(stby);
  servo.attach(servoPin);
}

void loop()
{
    abstand=sensor.Centimeters(); //auslesen des Ultraschallsensors
    Serial.print(abstand);
    Serial.println("cm");
   
    value=analogRead(speedPin);  //die Geschwindigkeit kann mit einem Potentiometer eingestellt werden
    roboSpeed=map(value,0,1023,200,255);
   
    motor.Move(1,roboSpeed,1); //vorwährts fahren
    motor.Move(2,roboSpeed,1);   
   
    if(abstand<10)//wenn der Roboter ein Hinderniss in 10cm entfernung entdeckt
    {
      motor.Stop(stby); // Roboter bleibt stehen
      servo.write(links);//Roboter sieht nach links
      if(sensor.Centimeters()<abstandLinks)//der kleinste abstand wird abgespeichert
      {
        abstandLinks=sensor.Centimeters();
      }
      delay(1500);
     
      servo.write(rechts);//Roboter sieht nach rechts
      if(sensor.Centimeters()<abstandRechts)//der kleinste abstand wird abgespeichert
      {
        abstandRechts=sensor.Centimeters();
      }
      delay(1500);
     
      servo.write(mitte);//Roboter sieht wieder nach vorne
     
      if(abstandRechts>abstandLinks)//wenn der abstand rechts größer als der abstand links ist
      {                             //dann soll der Roboter nach rechts fahren
        motor.Start(stby);
        motor.Move(2,roboSpeed,1);
        motor.Move(1,roboSpeed,2);
        delay(2000);
      }
      else if(abstandRechts<abstandLinks)//wenn der abstand rechts kleiner als links ist
      {                                  //dann soll er nach links fahren
        motor.Start(stby);
        motor.Move(1,roboSpeed,1);
        motor.Move(2,roboSpeed,2);
        delay(2000);
      }
    }     
}
Test Programm für Motoren(mit diesem Programm fährt der Roboter ohne Probleme vorwährts)
Code:
#include <Motor.h>

const int pwmA_rechts=10;  //rechter Motor
const int pwmB_links=11;   //linker Motor
const int AIN1=2, AIN2=3, BIN1=4, BIN2=5; //AIN1,AIN2,BIN1 und BIN2 sind die steuereingänge des Motortreibers ( ob er Vorwährts oder Rückwährts fahren soll)
const int stby=6;

Motor motor(pwmA_rechts,pwmB_links,AIN1,AIN2,BIN1,BIN2); //Motorinitialisierung

void setup()
{
  motor.Start(stby);
}

void loop()
{
  motor.Move(1,200,1);
  motor.Move(2,200,1);
  delay(10);
}

Motor-Library:
Motor.h
Code:
/*
Motor.h - to controll a Motor
written by Michael Köfinger, 4. Jänner 2013
*/
#ifndef Motor_h
#define Motor_h

#include "Arduino.h"

class Motor
{
    public:
            Motor(int PWM_A, int PWM_B, int AIN_1, int AIN_2, int BIN_1, int BIN_2);
            void Move(int motor, int motor_speed, int motor_direction);
            void Start(int stbyPin);
            void Stop(int stbyPin);

private:
int PWMA, PWMB,AIN1,AIN2,BIN1,BIN2;
};
#endif

Motor.cpp
Code:
/*
Motor.cpp - to controll a Motor
written by Michael Köfinger, 3. Jänner 2013
*/
#include "Arduino.h"
#include "Motor.h"

Motor::Motor(int PWM_A, int PWM_B, int AIN_1, int AIN_2, int BIN_1, int BIN_2)
{
PWMA = PWM_A; PWMB = PWM_B;
AIN1 = AIN_1; AIN2 = AIN_2; BIN1 = BIN_1; BIN2 = BIN_2;
pinMode(PWMA,OUTPUT);
  pinMode(PWMB,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
}

void Motor::Move(int motor, int motor_speed, int motor_direction)
{// motor: 1 oder 2(1: Motor A, 2: Motor B), speed: 0-255, direction: 1 oder 2 (1: vorwährts, 2: rückwährts)
 switch(motor)
 {
   case 1:
         switch(motor_direction)
         {
           case 1:
                 digitalWrite(AIN1,HIGH);
                 digitalWrite(AIN2,LOW);
                 break;
           case 2:
                 digitalWrite(AIN1,LOW);
                 digitalWrite(AIN2,HIGH);
                 break;
         }
         analogWrite(PWMA,motor_speed);
         break;
    case 2:
          switch(motor_direction)
          {
             case 1:
                   digitalWrite(BIN1,HIGH);
                   digitalWrite(BIN2,LOW);
                   break;
             case 2:
                   digitalWrite(BIN1,LOW);
                   digitalWrite(BIN2,HIGH);
                   break;
          }
         analogWrite(PWMB,motor_speed);
         break;
 }
}

void Motor::Start(int stbyPin)
{
digitalWrite(stbyPin,HIGH);
}

void Motor::Stop(int stbyPin)
{
digitalWrite(stbyPin,LOW);
}

mfg
Michael
Logged

Forum Moderator
BZ (I)
Offline Offline
Brattain Member
*****
Karma: 266
Posts: 21656
+39 349 2158303
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Ich kann keine Nennenswerten Unterschiede sehen.
Es bleibt Dir nichts anderes übrig als daß Du Programmteile stückchenweise auskommentierst ( Servo und Ultraschallsensor) und schaust ob der Fehler weg ist oder nicht.
Grüße Uwe
Logged

Austria
Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Danke für den Tipp!
Hab das auch gemacht. Ich hab ein testprogamm geschrieben mi dem der roboter nur die gabze zeit vorwährts fahren soll der servo sich von links nach rechtsbewegen soll und der sensor ausgelesen wird. Das hat auch funktioniert, nur als ich den teil mit der abfrage eingebaut hatte(ob der abstand kleiner als 10 cm ist) fahren die beiden motoren nicht mehr und der servo bewegt sich nur von links nach rechts ohne pause.
Habt ihr eine Idee was ich falsch gemacht habe?

mfg
Michael
Logged

Forum Moderator
BZ (I)
Offline Offline
Brattain Member
*****
Karma: 266
Posts: 21656
+39 349 2158303
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

bitte den funktionierenden und den nicht funktionierenden sketch
Grüße Uwe
Logged

Germany S-H
Offline Offline
Faraday Member
**
Karma: 146
Posts: 3039
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Habt ihr eine Idee was ich falsch gemacht habe?

Vorab: Ich kenne mich mit Servoprogrammierung am Arduino nicht aus, aber so eine grobe Vorstellung habe ich von Servos (habe vor x Jahren mal einige Modellbau-Fernlenkmodelle gebaut): Wenn einem Servo ein Steuerbefehl gegeben wird, dann blockiert der Steuerbefehl nicht die Programmausführung, bis der Servo seine Zielposition angefahren hat, sondern der Servo "stellt sich im Hintergrund" auf die befohlene Stellung, ohne das Programm zu blockieren.

Unter diesem Hintergrund schaue ich mal in Deinen Quellcode:

servo.write(links); // Servo soll mal langsam nach links drehen
if(sensor.Centimeters()<abstandLinks) // eine Mikrosekunde später (hat noch nichts gedreht)
{
   abstandLinks=sensor.Centimeters(); // Sensor ist aber hier noch in Geradeausposition!
}
delay(1500); // jetzt dreht er nach links

Ich sehe da nur: Die Abstands-Logik ist für den Eimer.

Schon eine (geschätzte) Mikrosekunde nach der Abgabe des Befehls "nun steuere mal langsam den Servo nach links" mißt Du einen Wert und weist ihn an abstandLinks zu. Dieser Wert wird immer ungefähr dieselbe Entfernung sein, in der sich auch das Hindernis befindet. Weil sich bei der Linksabstandsmessung der Servo tatsächlich noch in der Geradeausposition befindet.

Das delay(1500); nutzt der Servo, um nach links zu schwenken. Dort bekommt er nun ein:

servo.write(rechts); //Servo soll mal langsam nach rechts drehen (er ist aber noch links)
if(sensor.Centimeters()<abstandRechts)
{
  abstandRechts=sensor.Centimeters(); // es wird in Linksposition gemessen
}
delay(1500); // erst hier steuert der Servo nach rechts

D.h. eine Mikrosekunde nachdem der Steuerbefehl "nun mal nach rechts schwenken" kommt, steht der Servo noch voll am linken Anschlag und macht dort die Messung. abstandRechts ist nun tatsächlich der Abstand, der gemessen wird, wenn der Sensor voll nach links geschwenkt ist.

D.h. jedesmal beim Unterschreiten der 10 cm ist es so, dass
abstand <10 cm ist
abstandLinks <10 cm ist
abstandRechts ist auf dem Wert, der eigentlich bei abstandLinks gemessen werden sollte

Das ist das, was ich aus dem Code rauslesen kann, sorry, für mich sieht das in keiner Weise so aus, als wenn der tatsächlich programmierte Code das macht, was er laut den Kommentaren in Deinem Code machen soll.

Mein Vorschlag wäre, mal die delays zwischen "servo.write" und "sensor.Centimeters()" zu verschieben, etwa:

servo.write(links);//Steuerbefehl: Servo nach links
delay(1500);  // Zeit geben: Servo dreht nach links
abstandLinks=sensor.Centimeters(); // Messung Abstand links

Normalerweise sind Servos sehr viel schneller, so dass die delays stark verkürzt werden können. Selbst lahme Servos sollten für 90° Stellbereich (Mittelstellung bis Endstellung) nur 0,5s benötigen, für einen vollen Stellbereich von 180° (Endstellung zu entgegengesetzter Endstellung) max. 2*0,5= 1s. Vielleicht mal im Servo-Datenblatt nachsehen und die delay-Zeiten verringern. Aber eben die delays an der richtigen Stelle einfügen, so dass abstandLinks und abstandRechts gemessen werden, nachdem der Servo sich in die Richtung gedreht hat (und nicht bereits, wenn ihm gerade der Befehl gegeben wurde, dass er drehen soll).





« Last Edit: January 23, 2013, 06:06:01 pm by jurs » Logged

Austria
Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Danke für die Antworten!
Hier ist meine aktuelle Version von meinem Code
Code:
[code]#include <Servo.h>
#include <Ping.h>
#include <Motor.h>

#define TRUE 1
#define FALSE 0

const int PWMA_rechts=10;  //rechter Motor
const int PWMB_links=11;   //linker Motor
const int AIN1=2, AIN2=3, BIN1=4, BIN2=5; //AIN1,AIN2,BIN1 und BIN2 sind die steuereingänge des Motortreibers ( ob er Vorwährts oder Rückwährts fahren soll)
const int stby=6, pingPin=8, speedPin=0, servoPin=9;
const int mitte=89, links=0, rechts=179;
int roboSpeed=200, value;
long abstand, abstandLinks=1000, abstandRechts=1000;
Motor motor(PWMA_rechts,PWMB_links,AIN1,AIN2,BIN1,BIN2); //Motorinitialisierung
Ping sensor(pingPin);//Ultraschallsensorinitialisierung
Servo servo;
unsigned long start;
boolean hinderniss=FALSE;


void setup()
{
  Serial.begin(9600);
  pinMode(stby,OUTPUT);
  motor.Start(stby);
  servo.attach(servoPin);
  servo.write(mitte);
}

void loop()
{   
    motor.Move(1,roboSpeed,1); //vorwährts fahren
    motor.Move(2,roboSpeed,1);
   
    abstand=sensor.Centimeters(); //auslesen des Ultraschallsensors
    Serial.print(abstand);
    Serial.println("cm,");
   
    if(abstand<10)//wenn der Roboter ein Hinderniss in 10cm entfernung entdeckt
    {
      motor.Stop(stby); // Roboter bleibt stehen
      servo.write(links);//Roboter sieht nach links
      delay(100);
      start = millis();
      while(millis() - 2000 < start)
      {
        if(sensor.Centimeters()<abstandLinks)//der kleinste abstand wird abgespeichert
        {
          abstandLinks=sensor.Centimeters();
        }
      }
      servo.write(rechts);//Roboter sieht nach rechts
      delay(100);
      start = millis();
      while(millis() - 2000 < start)
      {
        if(sensor.Centimeters()<abstandRechts)//der kleinste abstand wird abgespeichert
        {
          abstandRechts=sensor.Centimeters();
        }
      }
      servo.write(mitte);//Roboter sieht wieder nach vorne
     hinderniss=TRUE;
    }
   
    if(hinderniss==TRUE)
    {
      if(abstandRechts>abstandLinks)//wenn der abstand rechts größer als der abstand links ist
      {                             //dann soll der Roboter nach rechts fahren#
        Serial.println(",rechts");
        motor.Start(stby);
        motor.Move(2,roboSpeed,1);
        motor.Move(1,roboSpeed,2);
        delay(2000);
      }
      else if(abstandRechts<abstandLinks)//wenn der abstand rechts kleiner als links ist
      {                                  //dann soll er nach links fahren
        Serial.println(",links");
        motor.Start(stby);
        motor.Move(1,roboSpeed,1);
        motor.Move(2,roboSpeed,2);
        delay(2000);
      }
      else
      {
        motor.Start(stby);
      }
      hinderniss=FALSE;
    }
}
[/code]
Hier ist das Video dazu:
Logged

Germany S-H
Offline Offline
Faraday Member
**
Karma: 146
Posts: 3039
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hier ist das Video dazu

Auch in Deinem neuen Sketch finde ich die Fahr-, Schwenk- und Drehlogik vom Ablauf her für den beabsichtigten Zweck nicht nachvollziehbar.

Noch ein Beispiel, was ich nicht nachvollziehen kann: Egal wie oft die loop durchlaufen wird und wie weit der Roboter fährt - die Werte für abstandLinks und abstandRechts können immer nur kleiner werden, niemals größer. Von der Logik her müßte es aber so sein: Wenn der Roboter ein Hindernis passiert hat und sich vom passierten Hindernis entfernt, dann müssen doch auch abstandLinks und abstandRechts irgendwann mal wieder größer werden als der minimale Abstand, der vor dem Hindernis zugewiesen wurde. Das findet in Deinem Sketch aber nicht statt. Bei Deinem Sketch ist es so: Je länger der Sketch läuft, können abstandLinks und abstandRechts mit zunehmender Laufzeit des Roboters nur kleiner werden. Das ist für mich ebenfalls völlig unlogisch. Vor allem wenn man bedenkt, dass der Roboter nach dem ersten Hindernis noch auf ein zweites Hindernis treffen könnte.
Logged

Pages: [1]   Go Up
Jump to: