RobotLib MotorCalibratie

Uit RoboWiki
Ga naar: navigatie, zoeken

Zie RobotLib Index voor een overzicht van alle RobotLib documentatie op deze wiki.

Inleiding

Op deze pagina wordt stap voor stap beschreven hoe de aansturing van motors en encoders wordt getest en gecalibreerd. Bij de start van dit stappenplan zijn de motoren en encoders aangesloten.

Stap 1 - HAL configuratie

Voor de aansturing van de motoren is een aantal routines nodig. Met een HAL wordt een template meegeleverd met de naam Prj_HAl_Motors.cpp. Deze file bevat de volgende functies:

void MotorInit();                           // initialiseer alle motor- en encoder-gerelateerde zaken, zoals GPIO, PWM en counters.
void MotorPwm(int PwmL, int PwmR);          // Stel motoren in op gegeven PWM waarde; range van 4095 (vol gas vooruit) tot -4096 (vol gas achteruit)
void EncoderRead (int &Left, int &Right);   // Lees encoders uit en geef het *verschil* terug

Deze functies worden door RobotLib gebruikt om de motoren aan te sturen en de encoders uit te lezen. Plaats de template file in de project directory en pas de file zo nodig aan om jouw robot te ondersteunen.

MotorPwm()

PFKeyMotorCalibrate1 heeft functie-toetsen waarmee PwmL en PwmR verhoogd en verlaagd kunnen worden, onafhankelijk van elkaar.

Gebruik deze functies om te bepaalen of MotorPwm() correct functioneert.

EncoderRead()

De routine EncoderRead() wordt door RobotLib aangeroepen om de encoders uit te lezen. Met het commando 'dset 7' wordt debug klasse 7 ingeschakeld. Deze meldt bij iedere aanroep van EncoderRead() het resultaat. Het commando 'dclr 7' zet deze vlag weer uit.

Gebruik deze functionaliteit om te controleren of de encoders correct functioneren. Voorwaarts bewegen moet een positieve waarde geven en achterwaarts bewegen geeft negatieve waarden.

Stap 2: PWM Calibratie

Bij deze stap worden de motoren aangestuurd op 75% en -25%. Stel de robot zo op dat de wielen vrij kunnen draaien. De reactie van de motoren wordt gemeten via de encoders. Na de meting worden per motor twee waarden berekend. Start de calibratie met het commando 'motorcal' op het console, of met PFKey 7 uit de set PFKeyMotorCalibrate1.

Het resultaat van de berekeningen wordt op het console geprint:

Result1 MotorL pwm: 3072.000000, speed: 547.599976, ratio 5.609934 
Result2 MotorL pwm: 1024.000000, speed: 150.557373, ratio 6.801394 
Result1 MotorR pwm: 3072.000000, speed: 528.960022, ratio 5.807622 
Result2 MotorR pwm: 1024.000000, speed: 148.377045, ratio 6.901337 
MotorL a: 5.158137, b: 247.404297 (247.404480)
MotorL a: 5.381218, b: 225.550781 (225.550781)
MotorL a: 5.158137, b: 343.551758 (258.650879)
#define MOTORL_SPEED_GAIN 516
#define MOTORL_OFFSET     301
#define MOTORR_SPEED_GAIN 538
#define MOTORR_OFFSET     226
Mission 'MSM_MotorCal' done

Neem de 4 regels met #define over in robotlib_conf.h.

De waarden van SPEED_GAIN moeten tussen de 100 en 1000 liggen. Als dit niet het geval is, wordt gevraagd de waarde van MOTOR_GAIN_SCALE in robotlib_conf.h aan te passen en de test opnieuw uitvoeren.

#define MOTOR_GAIN_SCALE    10   // Dit getal hoger maken als SPEED_GAIN <100 is

Stap 3: Afstand

Met de vorige set van PFKeys kan met 'pf 9' de volgende set PFKeys worden geactiveerd. Hiermee worden de PFKeys opnieuw geprogrammeerd naar PFKeyMotorCalibrate2.

Bij het geven van commando's wordt op het console onder meer de volgende gegevens geprint:

RobotPosition X: -6, Y: 0, Hoek: -162
OdoT_mm: 19

Gebruik de OdoT_mm variabele om de constante ODO_TICK_TO_METRIC in 'robotlib_conf.h' te tunen.

#define ODO_TICK_TO_METRIC      3568   // distance per tick, for the left wheel, 4096 means 1 tick => 1 mm (when ODO_TICK_HIRES_FACTOR == 1)      

De stapjes hiervoor zijn:

  • zet de robot op vast punt met voldoende ruimte voor zich, bijvoorbeeld met de achterkant tegen een muur.
  • 'reset robot-positie'
  • kies 'rij (ongeveer) rechtuit' en druk (bijvoorbeeld na een meter) op 'stop'
  • druk nogmaals op 'stop' als de robot stil staat. OdoT_mm geeft aan hoe ver de ROBOT heeft gereden volgens de encoders.
  • meet de gereden afstand met een ROLMAAT
  • corrigeer ODO_TICK_TO_METRIC, volgens de formule Nieuw = Oud * ROLMAAT / ROBOT

Herhaal voorgaande stappen om te bepalen of de calibratie binnen 10% nauwkeurig is. Als dit het geval is, verhogen we de nauwkeurigheid met de volgende stappen.

  • zet de robot op vast punt met voldoende ruimte voor zich, bijvoorbeeld met de achterkant tegen een muur.
  • 'reset robot-positie'
  • kies ' rij ca 1 meter vooruit'. De robot stopt automatisch na (iets meer dan) 1 meter.
  • Rij nogmaals een meter vooruit als er voldoende ruimte is (grotere afstand geeft een hogere nauwkeurigheid).
  • druk op 'stop' als de robot (voor de laatste keer) is gestopt. OdoT_mm geeft aan hoe ver de ROBOT heeft gereden volgens de encoders.
  • meet de gereden afstand met een ROLMAAT
  • corrigeer ODO_TICK_TO_METRIC, volgens de formule Nieuw = Oud * ROLMAAT / ROBOT

Herhaal dit totdat de gewenste nauwkeurigheid is bereikt. Bij variaties in de waarde ODO_TICK_TO_METRIC gelijk maken aan het gemiddelde van een aantal metingen.

De afstandcalibratie is nu gereed.

Stap 4: Hoek (1)

Meet de afstand tussen de twee wielen en bepaal de waarde van de constante ODO_HEADING met de formule:

ODO_HEADING = 917 * ODO_TICK_TO_METRIC / Wiel-basis (in mm)

waarbij

  • ODO_TICK_TO_METRIC = de waarde die we hierboven hebben bepaald.
  • Wiel-basis = de afstand tussen de twee wielen is, in mm
  • 917 = 360 * 8 / pi (of 360 * 256 * 256 / 4096 / 2 / pi)

De constante ODO_HEADING wordt gebruikt om de rijrichting van de robot (RobotHoek) te bepalen.

Een andere constante, ODO_TICK_L_R, wordt gebruikt om verschillen in wielgrootte te corrigeren. Om te beginnen zetten we deze op 4096, wat betekent dat beide wielen even groot zijn.

Geef de beide constantes in 'robotlib_conf.h' de gewenste waarde. In de volgende stappen van de calibratie worden deze waarden verder getuned.

#define ODO_TICK_L_R 4096 
#define ODO_HEADING  16360 // translate raw odo ticks to heading, larger => more degrees per ticks                                    
                           // Als de robot te weinig draait, moet dit getal lager worden

Stap 5: Hoek (2) en Wielgrootte

In de vorige stap is de afstand gecalibreerd en is een aanvang gemaakt met het calibreren van de rijrichting. In deze stap wordt de rijrichting calibratie verbeterd. Hierbij wordt ook gecorrigeerd voor een eventueel verschil in wielgrootte. De methode die we hiervoor gebruiken is UMBmark.

Met de functietoetsen in PFKeyMotorCalibrate2 kan de meting worden uitgevoerd voor een vierkant van 1 of 2 meter. Een groter vierkant geeft een hogere nauwkeurigheid. Het CLI menu 'TestCommands' bevat het commando 'umbmark' met de richting, de grootte van het vierkant en de maximale snelheid als parameters.

Opmerking: de variant van UMBmark, die zowel via het commando als de functietoetsen wordt aangeroepen, start en eindigt in het midden van een lange zijde.

De metingen:

  • Reset de robot positie en laat de robot een vierkant rijden met de klok mee (CW, ClockWise).
  • Noteer de afwijking ten opzichte van het startpunt (zie onderstaande figuur).
  • Herhaal de meting (eventueel meerdere keren) en bepaal de gemiddelde afwijking.
  • Reset de robot positie en laat de robot een vierkant rijden tegen de klok in (CCW, CounterClockWise).
  • Noteer de afwijking ten opzichte van het startpunt (zie onderstaande figuur).
  • Herhaal de meting (eventueel meerdere keren) en bepaal de gemiddelde afwijking.

Umbmark2.jpg

De correctie maken:

  • Zet de gemiddelde afwijking CW, de gemiddelde afwijking CCW, de grootte van het vierkant en de wielbasis van de robot in de cellen B1 t/m B4 van de [Excel sheet].
  • Zet de waarde van ODO_HEADING (uit Prj_RobotLib_conf.cpp) in cel B9.
  • Zet de waarde van ODO_TICK_L_R(uit Prj_RobotLib_conf.cpp) in cel B15.
  • Cel C9 bevat de nieuwe waarde voor ODO_HEADING, neem deze over in Prj_RobotLib_conf.cpp.
  • Cel C15 bevat de nieuwe waarde voor ODO_TICK_L_R, neem deze over in Prj_RobotLib_conf.cpp.

Herhaal dit totdat de gewenste nauwkeurigheid is bereikt.

Hiermee is de calibratie van de motoren en encoders afgerond en kan de robot met maximale nauwkeurigheid navigeren.