Menü
Aktuelles
Von:
Filter

Drehzahlermittlung in sunray

Silberstreifen

Silberstreifen

Active member
Ich bin vor einiger Zeit auf ein Thema gestoßen, dass ich mit Euch teilen möchte. Bei der Portierung der Hardware von der Ardumower PCB auf eine PICO-Version ist mir aufgefallen, dass die Drehzahlermittlung sehr ungenau ist.

In Sunray wir die Drehzahl durch Zählen der Impulse in einer festen Zeitspanne ermittelt. Dabei ist die Zeitspanne mit 50ms sehr kurz. Was zur Folge hat, dass nur sehr wenige Impulse gezählt werden. Bei einer Geschwindigkeit von 0,1m/s sind das beim Ardumower mit brushed Motoren lediglich 2 Impulse. Dabei entspricht ein Impuls 0,045m/s. Feiner kann die Drehzahl in Sunray nicht aufgelöst werden.

Bei den brushless Motoren mit 650 Ticks pro Umdrehung sieht es etwas besser aus. Ein Impuls entspricht 0,024m/s. Beim Alfred sind es 304 Ticks pro Umdrehung. Hier entspricht ein Impuls durch die kleineren Räder 0,042m/s.

Wenn der PID nun die Geschwindigkeit beim Ardumower mit brushed Motor bei 0,1m/s konstant halten soll, dann springt die ermittelte Drehzahl ständig zwischen 0,09 und 0,135m/s. Und der PID regelt entsprechend gegen. Was durchaus einen Beitrag zum drunken-Pilot-Verhalten leisten kann. Bei höheren Geschwindigkeiten springt die ermittelte Drehzahl genauso. Der PID muss also ständig gegenregeln.

Aus diesem Grunde habe ich die Drehzahlermittlung vom Zählen der Impulse auf das Messen der Zeitspanne zwischen den Impulsen umgestellt. Dadurch steigt die Auflösung und damit die Genauigkeit der Drehzahlermittlung von 0,045m/s auf 0,0009m/s. Wem das noch nicht reicht, der kann die Zeitspanne in Microsekunden messen. Was die Auflösung nochmals vertausendfacht.

Die erforderliche Änderung im Code ist nicht allzu aufwändig. In der AmRobotDriver.cpp muss folgendes angepasst werden
void AmMotorDriver::getMotorEncoderTicks(int &leftTicks, int &rightTicks, int &mowTicks, double &leftTickTime, double &rightTickTime, double &mowTickTime){
leftTicks = odomTicksLeft;
rightTicks = odomTicksRight;
mowTicks = odomTicksMow;
// reset counters
leftTickTime = ((double)odoLastTickTimeLeft - (double)odoFirstTickTimeLeft) / 1000.0 / 1000.0;
if(odomTicksLeft != 0)odoFirstTickTimeLeft = odoLastTickTimeLeft;
odomTicksLeft = 0;

rightTickTime = ((double)odoLastTickTimeRight - (double)odoFirstTickTimeRight) / 1000.0 / 1000.0;
if(odomTicksRight != 0)odoFirstTickTimeRight = odoLastTickTimeRight;
odomTicksRight = 0;
mowTickTime = ((double)odoLastTickTimeMow - (double)odoFirstTickTimeMow) / 1000.0 / 1000.0;
if(odomTicksMow != 0)odoFirstTickTimeMow = odoLastTickTimeMow;
odomTicksMow = 0;
}
Zum Vergrößern anklicken....

und
void OdometryMowISR(){
if (digitalRead(pinMotorMowRpm) == LOW) return;
if (millis() < motorMowTicksTimeout) return; // eliminate spikes
motorMowTicksTimeout = millis() + 1;
odoLastTickTimeMow = micros();
odomTicksMow++;
asm("dsb");
}
void OdometryLeftISR(){
if (digitalRead(pinOdometryLeft) == LOW) return;
if (millis() < motorLeftTicksTimeout) return; // eliminate spikes
motorLeftTicksTimeout = millis() + 1;
odoLastTickTimeLeft = micros();
odomTicksLeft++;
asm("dsb");
}
void OdometryRightISR(){
if (digitalRead(pinOdometryRight) == LOW) return;
if (millis() < motorRightTicksTimeout) return; // eliminate spikes
motorRightTicksTimeout = millis() + 1;
odoLastTickTimeRight = micros();
odomTicksRight++;
asm("dsb");

#ifdef TEST_PIN_ODOMETRY
testValue = !testValue;
digitalWrite(pinKeyArea2, testValue);
#endif
}
Zum Vergrößern anklicken....
In der motor.cpp sieht die Drehzahlermittlung dann so aus
if(ticksLeft != 0) motorLeftRpmCurr = (60.0 / (tickTimeLeft / (double)ticksLeft * (double)ticksPerRevolution));
if(ticksRight != 0) motorRightRpmCurr = (60.0 / (tickTimeRight / (double)ticksRight * (double)ticksPerRevolution));
if(ticksMow != 0) motorMowRpmCurr = (60.0 / (tickTimeMow / (double)ticksMow * (double)6.0));
Zum Vergrößern anklicken....

Mit dieser Änderung fahre ich jetzt seit 2 Wochen. Mein Eindruck ist, dass der Ardumower jetzt sehr viel ruhiger läuft und auch in der manuellen Steuerung sehr viel präziser geworden ist.
Vielleicht ist das ja etwas für die Mod-Version von @Mr. Tree
 
hab grad mal probiert Deinen Code einzufuegen, kann das sein dass die Deklaration von einigen Variablen wie tickTimeLeft etc. fehlen, da ich beim Kompilieren Fehler bekomme?!
Compilation error: 'tickTimeLeft' was not declared in this scope; did you mean 'ticksLeft'?
 
danke fuer die Dateien, auf die Schnelle hab ich das nicht eingepflegt, da sind ja etliche Aenderungen noch in den verschiedenen Dateien ohne genaue Angaben muss ich die Dateien erstmal in Ruhe vergleichen. Deine verwendetet Sunray ist ja auch nicht mehr die aktuellste aber trotzdem Danke
 
Vielen Dank auch von mir.
Die geänderte Drehzahlermittlung klingt für mich interessant, da ich einen Motor verwende, wo ich nicht viele Impule pro Umdrehung habe.
Die Änderungen werde ich bei Zeiten mal versuchen einzubauen.
 
Noch ein Hinweis: Die Änderungen in der Zip fokussieren auf den Ardumower. Für den Alfred müssen weitere Änderungen unter anderem in der RM18.ino vorgenommen werden. Bei Bedarf kann ich bei den Änderungen unterstützen. Testen kann ich es nicht, da ich keinen Alfred habe.
 
Die Drehzahl Berechnung um dann quasi wieder eine pwm Vorgabe über PID zu erzeugen um dann wieder aus den gemessenen Ticks den istwert der drehzahl zu für den PID zu bekommen ist absoluter Mist. Das ist eines der größten sunray Probleme. Für den langsamen original Alfred mit schlechter odometrie bedeutet es: funktioniert nicht. Beim ardumower geht es noch. Ich hab eine Version gebaut wo dieser recursive Kram rausgeflogen ist. Das Ergebnis ist phänomenal.Besonders für Alfred. Da habe ich wie Silberstreifen schon beschreibt auch die rm18 angepasst... Demnächst pushe ich das auch mal, aber erst wenn ich mit den obstacles durch bin.
 
Naja, ich denke es gibt einen Denkfehler im Ansatz. Auch bei deinem Kommentar. Es ist so, es ist uns egal wie viele Ticks wir sammeln solange sie nicht grob daneben liegen... Die Ticks sollten nur verwendet werden um zb. Eine Blockade oder im Vergleich mit dem Sollinput und der Abweichung zu einem Referenz wert eine schwergängigkeit festzustellen. Wir fahren nämlich hauptsächlich mit dem Gyro. Wenn der track nicht stimmt, regelt der Controller angular nach. Was interessieren uns dann Ticks? Es wird einfach mehr Drehung gegeben, um auf dem track zu bleiben. Hang ist also quasi Wurst. Wenn du dir das mal durch den Kopf gehen lässt, und lange drüber nachdenkst... Kommst du evtl. Wie ich zu dem Schluss das die ganze pwm aus ticks zu RPM Berechnung absoluter Blödsinn ist. Jedenfalls in unserem Hardware fall.
 
Ich meine wenn er sich eingegraben hat, sollte er doch trotzdem wegen den gesammelten Ticks eigentlich woanders sein? Nö. So "denkt" aber der Code. Hat sich doch gedreht....
 
Im Prinzip muss der Code aufgeräumt werden: kein rtk Roboter ohne Gyro. Ende Punkt aus. Die ganzen Varianten weg .. Gyro Pflicht, Referenz ist GPS heading gegen Drift und Initial. Weiterhin darf im stateestimator die GPS Referenz zur Bewegungsrichtung einbezogen werden wenn ein sehr hoher angular wert (45 Grad/s) unterschritten ist. Das ist viiieeel zu hoch? Wenn der Mäher einmal schwingt, dann schwingt die referenzierung genauso und er ist quasi locked im swing. Der Gyro driftet dir ja nicht in 10 Sekunden weg... Er driftet auch nicht in 30 Sekunden weg. Die Dinger sind super genau! Zb. T-Rex Helicopter Gyro um das tail zu stabilisieren... Gy50 irgendwas, das funktionierte schon vor 10 Jahren super! Und wir referenzieren hier ständig mit geringer Gewichtung den Gyro... Absolut unnötig und fehleranfällig. Stell dir mal vor, du hast einen kleinen fix GPS JUMP, Zack wird dein heading versaut!
 
Zuletzt bearbeitet:
no, but basically yes. the geared drives are so strong... they wont be hold but dig in the ground. So yes, simply set pwm value with an rpmtopwm factor, the rpm still is calculated over your wheel diameter. in final test, see that gps speed on a long straight line matches the input speed value. thats all. nothing else needed. The question is wrong, we dont forget the actual speed the mower is going, if its digging or the mower cant do it becouse of stuck or high load, speed doesnt matter. speed is never considered before in orig code, it did depend on defined values, its a setpoint calculated with multiple constants and then recursive measured over the tick count! the tick count varys each cylce of code, especially when you have higher iteration time or significant lower iteration time... and if the constants are definied wrong you never get the speed you did set up. its very simple to jack up the mower, let it run in test vor 10sec at pwm 100 or 150 and measure the wheel rounds in that time.. than transform it to the factor so a given set of pww equals rpm setpoint ... what is then actual speed. nothing else is needed. there are absolutely no ticks needed for that. And if rpm is calculated over the measured tick count: consider that --> have a unstable iteration time, maybe httpserver while loop is taking long... or you have standard jitter... rpm would be hard calculated on those ticks on iteration time which varies but is not considered... if you have a tick countdelta per iteration of 10 or 14, whats the speed the wheel is doing... to transform in in rpm and then transform it back to a setpoint pwm over PID.... do the math.
 
Zuletzt bearbeitet:
I have another rant: alfred did not work, because of that. it was filtered to death because the measured odometrie ticks (over unstable serial string connection!!!) were used to set the new pwm values of the drivers (over the serial connection xD). It´s a absolute desaster.
 
Silberstreifen schrieb:
Ich bin vor einiger Zeit auf ein Thema gestoßen, dass ich mit Euch teilen möchte. Bei der Portierung der Hardware von der Ardumower PCB auf eine PICO-Version ist mir aufgefallen, dass die Drehzahlermittlung sehr ungenau ist.
Zum Vergrößern anklicken....
It's important to have a correct odometry and mower platform setting to avoid issue and have smooth drive on the grass.
I made here so many change in 2 years that i don't know if i can explain everything here but:

To check everything the test code 10 rev is not enough , you need to add more test based on drive distance and roll one.
These 3 test need to have at least 10 correct result to avoid strange behaviour on mowing.
Roll test is used to check the distance between wheel value.

Into comm.cpp you can add 2 new test with E2 and E3
Code: 
void cmdMotorTest(){
  String s = F("E1");
  cmdAnswer(s);
  motor.test(); 
}

void cmdMotorRollTest(){
  String s = F("E2");
  cmdAnswer(s);
  motor.rollTest(); 
}

void cmdMotorDistanceTest(){
  String s = F("E3");
  cmdAnswer(s);
  motor.distanceTest(); 
}

and into motor.cpp
Code: 
void Motor::test(){
  CONSOLE.println("motor test - 10 revolutions");
  motorLeftTicks = 0; 
  motorRightTicks = 0; 
  unsigned long nextInfoTime = 0;
  int seconds = 0;
  int pwmLeft = 200;
  int pwmRight = 200;

  bool slowdown = true;
  long stopTicks = ticksPerRevolution * 10;
  unsigned long nextControlTime = 0;
  while (motorLeftTicks < stopTicks || motorRightTicks < stopTicks){
    if (millis() > nextControlTime){
      nextControlTime = millis() + 20;
      if ((slowdown) && ((motorLeftTicks + ticksPerRevolution  > stopTicks)||(motorRightTicks + ticksPerRevolution > stopTicks))){  //Letzte halbe drehung verlangsamen
        pwmLeft = pwmRight = 20;
        slowdown = false;
      }   
      if (millis() > nextInfoTime){     
        nextInfoTime = millis() + 1000; 
        if (seconds > 50)
      {
        break;
      }         
        dumpOdoTicks(seconds);
        seconds++;     
      }   
      if(motorLeftTicks >= stopTicks)
      {
        pwmLeft = 0;
      } 
      if(motorRightTicks >= stopTicks)
      {
        pwmRight = 0;     
      }
      
      speedPWM(pwmLeft, pwmRight, 0);
      sense();
      //delay(50);         
      watchdogReset();
      robotDriver.run();
    }
  } 
  speedPWM(0, 0, 0);
  CONSOLE.println("motor 10 rev done - please ignore any IMU/GPS errors");
  CONSOLE.println("ADJUST TICKS_PER_REVOLUTION into config.h is not OK");
}


void Motor::distanceTest(){
  CONSOLE.println("3 meters motor test");
  motorLeftTicks = 0; 
  motorRightTicks = 0; 
  unsigned long nextInfoTime = 0;
  int seconds = 0;
  int pwmLeft = 150;
  int pwmRight = 150;
  bool slowdown = true;
  long stopTicks = ticksPerCm * 300;
  unsigned long nextControlTime = 0;
  while (motorLeftTicks < stopTicks || motorRightTicks < stopTicks){
    if (millis() > nextControlTime){
      nextControlTime = millis() + 20;
      if ((slowdown) && ((motorLeftTicks + ticksPerRevolution  > stopTicks)||(motorRightTicks + ticksPerRevolution > stopTicks))){  //Letzte halbe drehung verlangsamen
        pwmLeft = pwmRight = 80;
        slowdown = false;
      }   
      if (millis() > nextInfoTime){     
        nextInfoTime = millis() + 1000;
        if (seconds > 50)
      {
        break;
      }           
        dumpOdoTicks(seconds);
        seconds++;     
      }   
      if(motorLeftTicks >= stopTicks)
      {
        pwmLeft = 0;
      } 
      if(motorRightTicks >= stopTicks)
      {
        pwmRight = 0;     
      }
      
      speedPWM(pwmLeft, pwmRight, 0);
      sense();
      //delay(50);         
      watchdogReset();
      robotDriver.run();
    }
  } 
  speedPWM(0, 0, 0);
  CONSOLE.println(" 3 meters done - please ignore any IMU/GPS errors");
  CONSOLE.println("ADJUST WHEEL_DIAMETER into config.h is not OK");
}


void Motor::rollTest(){
  CONSOLE.println("motor roll test - 360 deg rotation");
  motorLeftTicks = 0; 
  motorRightTicks = 0; 
  unsigned long nextInfoTime = 0;
  int seconds = 0;
  int pwmLeft = 150;
  int pwmRight = -150;
  motorLeftPWMCurr =100;
  motorRightPWMCurr =-100; // need a negative number to correctly compute the odometry on reverse run

  bool slowdown = true;
 
  //bber100
  long stopTicksLeft =  (int)36000 * (ticksPerCm * PI * wheelBaseCm / 36000);           
  long stopTicksRight = -(int)36000 * (ticksPerCm * PI * wheelBaseCm / 36000);

  CONSOLE.println(stopTicksLeft);
  CONSOLE.println(stopTicksRight);

  unsigned long nextControlTime = 0;
  while (motorLeftTicks < stopTicksLeft || motorRightTicks > stopTicksRight){
    if (millis() > nextControlTime){
      if (seconds > 50)
      {
        break;
      }
      nextControlTime = millis() + 20;
      if ((slowdown) && ((motorLeftTicks + ticksPerRevolution  > stopTicksLeft)||(motorRightTicks + ticksPerRevolution > stopTicksRight))){  //Letzte halbe drehung verlangsamen
        pwmLeft = 80;
        pwmRight = -80;
        slowdown = false;
      }   
      if (millis() > nextInfoTime){     
        nextInfoTime = millis() + 1000; 
        dumpOdoTicks(seconds);
        seconds++;     
      }   
      if(motorLeftTicks >= stopTicksLeft)
      {
        pwmLeft = 0;
      } 
      if(motorRightTicks <= stopTicksRight)
      {
        pwmRight = 0;     
      }
      speedPWM(pwmLeft, pwmRight, 0);
      sense();
      //delay(50);         
      watchdogReset();
      robotDriver.run();
    }
  } 
  speedPWM(0, 0, 0);
  CONSOLE.println("Roll 360 Degree done - please ignore any IMU/GPS errors");
  CONSOLE.println("ADJUST WHEEL_BASE_CM into config.h is not OK");
  }

Also into mower.h
motorLeftticks are declare as unsigned , but it's not OK because wheel can rotate reverse and negative possible value
Code: 
 bool pwmSpeedCurveDetection;
    long motorLeftTicks;
    long motorRightTicks;
    long motorMowTicks;

Dumpodoticks is also not OK:
Code: 
void Motor::dumpOdoTicks(int seconds){
  int ticksLeft=0;
  int ticksRight=0;
  int ticksMow=0;
  motorDriver.getMotorEncoderTicks(ticksLeft, ticksRight, ticksMow); 
  if (motorLeftPWMCurr < 0) ticksLeft *= -1;
  if (motorRightPWMCurr < 0) ticksRight *= -1;
  if (motorMowPWMCurr < 0) ticksMow *= -1;
  motorLeftTicks += ticksLeft;
  motorRightTicks += ticksRight;
  motorMowTicks += ticksMow;
  CONSOLE.print("t=");
  CONSOLE.print(seconds);
  CONSOLE.print("  ticks Left=");
  CONSOLE.print(motorLeftTicks); 
  CONSOLE.print("  Right=");
  CONSOLE.print(motorRightTicks);             
  CONSOLE.print("  current Left=");
  CONSOLE.print(motorLeftSense);
  CONSOLE.print("  Right=");
  CONSOLE.print(motorRightSense);
  CONSOLE.println();               
}

So as you can see you need a lot of change to increase the working mode of odometry.



With correct Odometry and IMU mower can drive without fix for a long duration (it's AZURITBER version)
 
bernard schrieb:
With correct Odometry and IMU mower can drive without fix for a long duration (it's AZURITBER version)
Zum Vergrößern anklicken....
yes I fully agree. So my first step was to improve the odometry. My next step is to improve the PID.
Mr. Tree schrieb:
Im Prinzip muss der Code aufgeräumt werden:
Zum Vergrößern anklicken....
sehe ich genauso. Aufgrund der vielen Optionen verliert man leicht den Überblick und stößt nur sehr schwer auf die eigentlichen Probleme.
Ich habe mir in den letzten Wochen auch noch enmal die Theorie für den PID und für STANLEY durchgelesen. Dabei fällt auf, dass die Umsetzung in sunray nicht der Theorie entspricht. STANLEY habe ich bereits mit guten Ergebnissen neu programmiert. Der PID ist als nächstes dran, aber ohne PID wird es aus meiner Sicht nicht gehen.
 
If you figure out a better way to implement PID please do share.
I have the motor control loop running continously instead of in 50ms increments and then running pid everytime the the tickTime changes or I get new ticks, I feel like this makes an improvement.
C++: 
void Motor::control(bool updateLeft, bool updateRight, bool updateMow){
    ...
    }
It can still a bit notchy when the terrain changes abruptly, like starting to go downhill, but its pretty accurate.

Using just PWM would probably be pretty smooth, I dont know how accurate it would be on slopes though, I have some big hills in my yard, maybe taking pitch into account to increase/decreaae pwm on slopes?
 
WhatATest schrieb:
If you figure out a better way to implement PID please do share.
Zum Vergrößern anklicken....
I didn't change much, but the effect is nice.

In the motor.cpp:
motorLeftPWMCurr = motorLeftPID.y;

//motorLeftPWMCurr = motorLeftPWMCurr + motorLeftPID.y;
//if (motorLeftRpmSet >= 0) motorLeftPWMCurr = min( max(0, (int)motorLeftPWMCurr), pwmMax); // 0.. pwmMax
//if (motorLeftRpmSet < 0) motorLeftPWMCurr = max(-pwmMax, min(0, (int)motorLeftPWMCurr)); // -pwmMax..0
Zum Vergrößern anklicken....
and
motorRightPWMCurr = motorRightPID.y;

//motorRightPWMCurr = motorRightPWMCurr + motorRightPID.y;
//if (motorRightRpmSet >= 0) motorRightPWMCurr = min( max(0, (int)motorRightPWMCurr), pwmMax); // 0.. pwmMax
//if (motorRightRpmSet < 0) motorRightPWMCurr = max(-pwmMax, min(0, (int)motorRightPWMCurr)); // -pwmMax..0

//if ((abs(motorLeftRpmSet) < 0.01) && (motorLeftPWMCurr < 30)) motorLeftPWMCurr = 0;
//if ((abs(motorRightRpmSet) < 0.01) && (motorRightPWMCurr < 30)) motorRightPWMCurr = 0;

if ((abs(motorLeftRpmSet) < 0.01) && (abs(motorLeftPWMCurr) < 30)) motorLeftPWMCurr = 0;
if ((abs(motorRightRpmSet) < 0.01) && (abs(motorRightPWMCurr) < 30)) motorRightPWMCurr = 0;
Zum Vergrößern anklicken....

Due to that, you have to change the PID-parameters. For my brushed motors, I found the following:
#define MOTOR_PID_KP 0.0
#define MOTOR_PID_KI 50.0
#define MOTOR_PID_KD 0.0
Zum Vergrößern anklicken....
It may be, that the brushless motors or Alfred do need other parameter.

With this changes, my mower is much more responsive and the accuracy is also better. In first tests the lateral error was typically less than 2cm even on gradient routes.
 
Thats basically a pure pwm Ramp. Also, the p Factor is Generally a RPM to pwm Factor. The PID Controller IS somewhat overpowered in the Code. But its beneficial that IT handles runtime cycle. Still, In hate everything that needs Numbers in Code Like 0,001 .... or 0,0032
 
Zuletzt bearbeitet:
Du musst dich einloggen oder registrieren, um hier zu antworten.
Oben