Menü
Aktuelles
Von:
Filter

Umfrage Odometrie

Hallo,

die kleinen Änderungen in der pfod.cpp habe ich auch noch ins Git gepackt.

Ausserdem habe ich den OdometryTest in der seriellen Konsole um die Funktion "One Revolution" erweitert.
Damit kann man ganz schnell feststellen ob die Einstellungen der Odometrie korrekt sind.
Wenn man "o" drückt werden die Zähler gelöscht und es wird eine komplette Radumdrehung durchgeführt.

Bei mir funktioniert das bis jetzt ganz gut. Auch mit Perimeter. IMU konnte ich nicht mehr probieren, es regnet :(

VG
Reiner
 
Yes.
3 perfects test

1 1 or better 5 turns of Wheel at half speed so 10120 ticks to test the ticks/turn.
2 Non stop Forward and backward for 1ml so 27100 ticks for you to test the ticks/turn and 'repetitivity'.
3 Left Wheel 0 and 360 degres of rotate whith the right Wheel to test the Wheel distance.

Easy to say but possible B) .

By.
 
@Rainer danke ich werde es mal testen. Leider Regnet es hier gerade.

Bei den Interrupts könnte man da dann nicht nur rising verwenden um Interrupts zu sparen ?

Coole Idee mit der test Routine.

Stefan
 
Ich konnte es doch noch kurz draußen testen.

Bringt keine Fehler. Läuft nicht gerade aus, sondern in einem langgezogenem Bogen, als wenn die Odometrie ausgeschaltet ist.

Die pfodApp reagiert stark verzögert.
 
Hallo Stefan,
ist die Richtung des Bogens immer gleich?
drehen sich die Räder beim neuen ODO-Test für genau eine Umdrehung?
Gleiche Motoren bzw. Encoder? Pullup Widerstände an den ODO Eingängen?

VG
Reiner
 
Ja die Räder drehen genau eine Umdrehung allerdings, konnte ich in der pfodapp nicht genau auf 256 Ticks stellen, ist echt fummelig mit dem slider aber test ist gut zum prüfen ob alles ok ist.

Der Bogen ist geht nach links und ist immer gleich mit und ohne odometrie. Bei dem kuzen testen konnte ich auch keine Korrekturversuche beobachten.

Ich stöbere gerade im Code rum und dabei auf eine Funktion gestoßen, die anscheinend nirgens aufgerufen wird .
In der robot.cpp gibt es eine Robot::calcOdometry() dort wird die Odometrie auch berechnet, jedoch finde ich nirgens einen Aufruf. Ist das ein Überbleibsel ?

Stefan
 
Es wird einmal die aktuelle Drehzahl jedes Rades berechnet:


Code: 
motorLeftRpmCurr  = double ((( ((double)ticksLeft) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0); 
  motorRightRpmCurr = double ((( ((double)ticksRight) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0);

und jeweils die X und Y Position des Mowers:

Code: 
odometryX += avg_cm * sin(odometryTheta); 
  odometryY += avg_cm * cos(odometryTheta);
 
Das die Position dort berechnet wird hatte ich gesehen aber auch die Motorsteuerung wird dort berechnet.


Code: 
motorLeftRpmCurr  = double ((( ((double)ticksLeft) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0); 
motorRightRpmCurr = double ((( ((double)ticksRight) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0);
 
Moin,
[strike]
nach einigen Tests denke ich auch dass die ODO nicht das Problem ist. Die "verbraucht" in der aktuellen Dev. Version nur ca. 2300 Loops/Sek.
Aber wie Boilevin schon geschrieben hat, schlägt die IMU mit ca. 12000 Loops/Sek. zu. Ist ja auch verständlich, muss halt viel gerechnet werden...[/strike]

nach einigen Tests denke ich auch dass die ODO nicht das Problem ist. Die "verbraucht" jetzt nur ein Paar Loops/Sek.
Die IMU schlägt anscheinend mit etwas über 5000 Loops/sek. zu.

VG
Reiner
 
Hi all

Here the code to test all the odometry.
Simply replace the existing fonction in Robot.cpp

The test are now:
'f' forward"));
'r' reverse"));
'z' reset Count"));
'a' One wheel revolution forward "));
'b' Five wheel revolution forward"));
'c' One meter forward and backward non stop wheel stop only when ticks count reach Use to control the ticks/cm"));
'd' One Rotation 360 Left Use to control the 2 wheel distance WheelBaseCm "));
'e' One Rotation 360 Rigt Use to control the 2 wheel distance WheelBaseCm "));
'g' Increase PWM speed test, need to be set before starting the test or test at half max speed "));


Code: 
void Robot::testOdometry() {

  char testCurrent; // use to know if the test is non stop or need to stop  and for console printing
  char ch;
  int lastLeft = 0;
  int lastRight = 0;
  unsigned long StopRightAt;
  unsigned long StopLeftAt;
  int TicksStopLeft = 30000; //use to stop the test
  int TicksStopRight = 30000;
  float MotorLeftPWMTest;  // Use this variable instead of motorLeftPWMCurr to avoid change by the motor protection
  float MotorRightPWMTest;  // Use this variable instead of motorLeftPWMCurr to avoid change by the motor protection
  MotorLeftPWMTest = motorSpeedMaxPwm / 2;
  MotorRightPWMTest = motorSpeedMaxPwm / 2;
  odometryLeft = 0; odometryRight = 0;
  resetIdleTime();
  Console.println(F("Press 'f' forward"));
  Console.println(F("Press 'r' reverse"));
  Console.println(F("Press 'z' reset Count"));
  Console.println(F("Press 'a' One  wheel revolution forward "));
  Console.println(F("Press 'b' Five wheel revolution forward"));
  Console.println(F("Press 'c' One meter forward and backward non stop wheel stop only when ticks count reach Use to control the ticks/cm"));
  Console.println(F("Press 'd' One Rotation 360 Left    Use to control the 2 wheel distance WheelBaseCm "));
  Console.println(F("Press 'e' One Rotation 360 Rigt    Use to control the 2 wheel distance WheelBaseCm  "));
  Console.println(F("Press 'g' Increase PWM speed test, need to be set before starting the test or test at half max speed "));
  Console.println(F(" "));
  Console.println (F("Press '0' To Stop"));
  Console.println(F(" "));

  while (true) {
    if ((odometryLeft != lastLeft) || (odometryRight != lastRight)) {
      if (testCurrent == 'f') Console.print(F("Non Stop Forward       "));
      if (testCurrent == 'r') Console.print(F("Non Stop Backward      "));
      if (testCurrent == 'a') Console.print(F("One wheel revolution   "));
      if (testCurrent == 'b') Console.print(F("Five wheel revolution  "));
      if (testCurrent == 'c') Console.print(F("Non stop 1 Meter       "));
      if (testCurrent == 'd') Console.print(F("Rotation 360 Left      "));
      if (testCurrent == 'e') Console.print(F("Rotation 360 Righ      "));
      Console.print(F("PWM = "));
      Console.print(MotorLeftPWMTest);
      Console.print(F("  left="));
      Console.print(odometryLeft);
      Console.print(F("  right="));
      Console.print(odometryRight);
      Console.print(F(" Left Cible "));
      Console.print(TicksStopLeft);
      Console.print(F(" Right Cible "));
      Console.println(TicksStopRight);
      lastLeft = odometryLeft;
      lastRight = odometryRight;
    }
    delay(10); //need to test fast to stop correctly
    if ((testCurrent == 'a') || (testCurrent == 'b') || (testCurrent == 'd') || (testCurrent == 'e')) {  // All  test need to stop when reach ticks wanted
      if ((odometryLeft >= TicksStopLeft) || (odometryRight >= TicksStopRight)) { //test if we reach the ticks wanted then stop the wheel
        MotorLeftPWMTest = 0; MotorRightPWMTest = 0;
        setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
        Console.println(F(" "));
        Console.print (F("Test  Finish at "));
        Console.print(F("left="));
        Console.print(odometryLeft);
        Console.print(F("  right="));
        Console.print(odometryRight);
        Console.println(F(" "));
        odometryLeft = 0; odometryRight = 0;
        lastLeft = 0; lastRight = 0;
        TicksStopLeft = 30000;
        TicksStopRight = 30000;
        
        break;
      }
    }
    if (testCurrent == 'c')  {
      if ((millis() > StopLeftAt + 2000) && (StopLeftAt != 0)) { // wait 2 sec for the other wheel finish count
        Console.println(F("Restart left"));
        setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
        StopLeftAt = 0;
      }
      if ((millis() > StopRightAt + 2000) && (StopRightAt != 0)) { // wait 2 sec for the other wheel finish count
        Console.println(F("Restart Right"));
        setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
        StopRightAt = 0;
      }
      if (abs(odometryLeft) >= abs(TicksStopLeft)) {
        StopLeftAt = millis();
        odometryLeft = 0;
        lastLeft = 0;
        MotorLeftPWMTest = -1 * MotorLeftPWMTest;
        setMotorPWM(0, MotorRightPWMTest, true);
        Console.println (F("Stop left wheel"));
      }
      if (abs(odometryRight) >= abs(TicksStopRight)) {
        StopRightAt = millis();
        odometryRight = 0;
        lastRight = 0;
        MotorRightPWMTest = -1 * MotorRightPWMTest;
        setMotorPWM(MotorLeftPWMTest, 0, true);
        Console.println (F("Stop Right wheel"));
      }
    }
    //read the console
    if (Console.available() > 0) {
      ch = (char)Console.read();
      switch (ch) {
        case '0':
          MotorLeftPWMTest = 0; MotorRightPWMTest = 0;
          setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
          return;
        case 'a':
          testCurrent = 'a';
          setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
          odometryLeft = 0; odometryRight = 0;
          TicksStopLeft = odometryTicksPerRevolution;
          TicksStopRight = odometryTicksPerRevolution;
          break;
        case 'b':
          testCurrent = 'b';
          setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
          odometryLeft = 0; odometryRight = 0;
          TicksStopLeft = 5 * odometryTicksPerRevolution;
          TicksStopRight = 5 * odometryTicksPerRevolution;
          break;
        case 'c':
          testCurrent = 'c';
          Console.println(F(" "));
          Console.println (F("  '0' To stop the test "));
          Console.println(F(" "));
          setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
          odometryLeft = 0; odometryRight = 0;
          TicksStopLeft =  odometryTicksPerRevolution;
          TicksStopRight =  odometryTicksPerRevolution;
          break;
        case 'd':
          testCurrent = 'd';
          setMotorPWM(MotorLeftPWMTest, 0, false);
          odometryLeft = 0; odometryRight = 0;
          TicksStopLeft =  (int)(2 * PI * odometryWheelBaseCm * odometryTicksPerCm);
          TicksStopRight = 30000;
          break;
        case 'e':
          testCurrent = 'e';
          setMotorPWM(0, MotorRightPWMTest, false);
          odometryLeft = 0; odometryRight = 0;
          TicksStopRight =  (int)(2 * PI * odometryWheelBaseCm * odometryTicksPerCm);
          TicksStopLeft = 30000;
          break;
        case 'f':
          Console.println(F(" "));
          Console.println (F("  '0' To stop the test "));
          Console.println(F(" "));
          testCurrent = 'f';
          setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
          break;
        case 'g':
          MotorLeftPWMTest = MotorLeftPWMTest + 50;
          if (MotorLeftPWMTest > 250) MotorLeftPWMTest = 50;
          MotorRightPWMTest = MotorLeftPWMTest;
          Console.print(F(" New speed is now : "));
          Console.println (MotorLeftPWMTest);
          Console.println(F(" "));
          break;
        case 'r':
          Console.println(F(" "));
          Console.println (F("  '0' To stop the test "));
          Console.println(F(" "));
          testCurrent = 'r';
          setMotorPWM(-MotorLeftPWMTest, -MotorRightPWMTest, false);
          break;
        case 'z':
          testCurrent = 'z';
          odometryLeft = 0; odometryRight = 0;
          break;
      }
    }
  }
  MotorLeftPWMTest = 0; MotorRightPWMTest = 0;
  setMotorPWM(MotorLeftPWMTest, MotorRightPWMTest, false);
}


Hope it's works.

Sorry Alexander I view you sugest Github but don't know how i can.

Have a good day
 
[strike]...So wie es aussieht sind es gar nicht die Berechnungen in der IMU die das System ausbremsen, sondern die Kommunikation über I2C.

Wenn ich die Read Funktionen auskommentiere, dann läufts weiter mit ca. 13000 loops/Sek.

IMU::read

Code: 
readL3G4200D(true);
  readADXL345B();
  readHMC5883L();
[/strike]

Also... die Berechnung bei der IMU dauert ca. 2ms und das lesen der Daten ca. 2,5ms
 
Hi Reiner,
ich bin gerade etwas verwundert wie du es schaffst auf 13000 loops/sec zu kommen :huh:
Ich hab im Leerlauf knapp 3000 und im Betrieb mit 1Way Odo und ohne IMU Werte runter bis 1200.

Wo liest du die loops/sec eigentlich ab?
 
Hab jetzt auch mal die Konsole angeworfen(schau normal nur pfod werte) und hab einen Verdacht :woohoo:
Da steht ein "l" (steht wohl für loop) vor dem eigentlichen Wert. Das könnte erklären wieso mein MEGA gar so langsam ist :huh:
 
Hallo Alex,

ja, jetzt wo Du´s sagst seh ich´s auch :silly:
Das "l" und die "1" sehen sich aber auch zum Verwechseln ähnlich...

Aber das ändert ja nix an der Sache...

VG
Reiner
 
Moin,

also, die ODO belastet das System jetzt nicht so stark wie vermutet. Ohne ODO braucht ein Schleifendurchlauf ca. 0,4ms, mit ODO ca. 0,6ms.
Wenn ich die IMU einschalte, dann braucht der Loop ca. 4-5ms. Dabei brauchen IMU.read() und IMU.update() etwa jeweils die Hälfte der Zeit.

VG
Reiner
 
Hi all.
In my last post i put a code with other odometry test.
All the test work well but The test 'c' show Something Strange.

The mower move each Wheel 2000 ticks for example and reverse so normaly a straight line.

In fact in my case the two Wheel don't have exactly the same speed and the result is very Strange :huh:

Did everyone can make the test and tell me if it's the same thing with ardumower shop motors.

If yes maybee it's a good think to put an offset in one of the two pwm motor to have exactly the same speed left and right and it's help the pid motor to work better.

Thanks.
 
Du musst dich einloggen oder registrieren, um hier zu antworten.
Oben