RobotLib MotorCalibratie
Zie deze pagina 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.
Voor de software nemen we Sample_Project_1 van je robotcontroller als uitgangspunt.
Stap 1 - HAL configuratie
Voor de aansturing van de motoren is een aantal routines nodig. Sample_Project_1 bevat een template 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. 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. PFKeyMotorCalibrate1 is één van de Programmable_Function_Key handlers die wordt geladen in het sample project. Je kunt een lijst met geladen PFKeyHandlers opvragen met:
F4Bot> pflist 0 PFKeyMotorCalibrate1 1 PFKeyMotorCalibrate2 2 * PFKeyUmbMark 3 PFKeyUmTest1 4 PFKeyUmTest2 F4Bot>
Het sterretje voor PFKeyUmbMark geeft aan dat dit de active PFKeyHandler is. PFKeyMotorCalibrate1 is handler nummer 0 en wordt geactiveerd met het commando 'pfset 0':
F4Bot> pfset 0 F4Bot> pflist 0 * PFKeyMotorCalibrate1 1 PFKeyMotorCalibrate2 2 PFKeyUmbMark 3 PFKeyUmTest1 4 PFKeyUmTest2 F4Bot>
Let op: na reset is de default PFKeyHandler weer actief. Dit is de laatste PFKeyHandler die is aangeroepen in je code. De standaard code in main.cpp ziet er als volgt uit:
//-------------------------------------------------------------------------- // Setup PFKey handers SG_SetupDefaultFfKeyHandlers(); PFKeyHandlerSet(FP_FNAME(PFKeyUmbMark)); // zie Prj_UmbMark.cpp
Door de volgende regel toe te voegen, wordt PFKeyMotorCalibrate1 de default PFKeyHandler:
//-------------------------------------------------------------------------- // Setup PFKey handers SG_SetupDefaultFfKeyHandlers(); PFKeyHandlerSet(FP_FNAME(PFKeyUmbMark)); // zie Prj_UmbMark.cpp PFKeyHandlerSet(FP_FNAME(PFKeyMotorCalibrate1));
We kunnen nu met de eerste 4 functies van PFKeyMotorCalibrate1 controleren of MotorPwm() correct functioneert.
- Met PF1 gaat de linkse motor vooruit draaien. Met iedere druk op PF1 gaat de linkse motor sneller vooruit.
- Met PF2 vertraagt de linkse motor. Met iedere druk op PF2 vertraagt de linkse motor meer, totdat deze tot stilstand komen en achteruit gaat draaien.
- PF3 en PF3 doen hetzelfde, maar dan voor de rechtse motor.
Let op:
- Controleer de werking van een motor ook, terwijl de andere langzaam vooruit en langzaam achteruit draait, om zeker te zijn dat de motoren elkaar niet beïnvloeden.
- De boven beschreven stapjes zijn steeds een verandering van 512 van de PWM waardes, de lopen van +4095 (volle snelheid vooruit) tot -4095 (volle snelheid achteruit).
- De PWM waarden worden uitgeprint op het console (de serial poort).
- Rond het nulpunt zijn twee stapjes extra in de reeks, PWM 1 en PWM -1. Deze zijn handig bij het testen als bij het omschakelen van de draairichting de snelheid maximaal wordt.
Als de motoren niet naar wens reageren, pas dan Prj_Hal_Motors.cpp aan.
EncoderRead()
Robotlib leest de encoders uit met een aanroep van de routine EncoderRead().
Met het commando 'dlevel 7 1' wordt debug klasse 7 op niveau 1 (warning) gezet. Als de robot beweegt (encoder waarden ongelijk aan 0), meldt deze klasse op niveau 1 het resultaat van EncoderRead() als er een wiel beweegt. Dit kan met de PFKeys die we hiervoor gebruikt hebben, of door de wielen met de hand te verdraaien.
[1.7-C:\RobotLib\RobotLib\Position.cpp:133] DeltaL: 50844 DeltaR: 0 RobotXPos: -2090 RobotYPos: -421 RobotHoek: 32
In bovenstaand voorbeeld geeft DeltaL een positieve waarde, wat aangeeft dat het linkse wiel (volgens de encoder) vooruit draait. De waarde van DeltaR is 0, dus dat wiel staat stil. De overige gegevens zijn nu niet relevant.
Als de encoders niet de gewenste waarden geven, kun je dit aanpassen in prj_hal_motors.cpp. Hieronder staat de code die de tellers van de encoders uit. De waarden van ENCL_TIMER en ENCR_TIMER bepalen welke teller gebruikt wordt. De waarden worden opgegeven in prj_hal_conf.h en zijn daar eenvoudig om te wisselen. (Het gebruik van andere tellers vraagt wellicht meer aanpasingen.)
// read encoders LeftEncoder = TIM_GetCounter (ENCL_TIMER) ; // flip sign here if required RightEncoder = TIM_GetCounter (ENCR_TIMER) ; // flip sign here if required
De draairichting kun je aanpassen door een min-teken toe te voegen, bijvoorbeeld voor het linkse wiel:
// read encoders LeftEncoder = -TIM_GetCounter (ENCL_TIMER) ; // flip sign here if required RightEncoder = TIM_GetCounter (ENCR_TIMER) ; // flip sign here if required
(Dit voorbeeld is van de STM32F4. De code voor andere controllers zal afwijken).
Stap 2: Afstand (grof)
Eerst gaan we de orde grootte van de afstand goed zetten
- Zet de robot met de wielen van de vloer.
- Geeft het commando 'positionreset'
- Draai beide wielen (zo mogelijk tegelijk) 1 omwenteling vooruit
- Geef het commando 'position'
RobotPosition X: 204, Y: 1, Hoek: -1 (198) GreenBot>
Omdat beide wielen ongeveer evenveel gedraaid zijn, moet de hoek ongeveer 0 zijn. Ook Y zal klein zijn als beide wielen gelijk gedraaid zijn. X geeft de afstand aan, die de robot heeft gemeten. Deze afstand wordt bepaald door:
#define ODO_TICK_TO_METRIC_F 0.04 // distance per tick, in mm
- Meet de diameter van de wielen (in bovenstaand voorbeeld is deze 71mm).
- De omtrek is 71 * 3.14 = 223mm
- De nieuwe waarde van ODO_TICK_TO_METRIC_F = 0.0404 / 204 * 223 = 0.0437
Herhaal voorgaande stappen tot de calibratie binnen 20% nauwkeurig is.
Stap 3: Hoek (grof)
Meet de afstand tussen de twee wielen en bepaal de waarde van de constante ODO_HEADING_F met de formule:
ODO_HEADING_F = ODO_TICK_TO_METRIC_F / Wiel-afstand
waarbij
- ODO_TICK_TO_METRIC_F = de waarde die we hierboven hebben bepaald.
- Wiel-afstand = spoor-breedte, de afstand tussen de twee wielen is, in mm
De constante ODO_HEADING_F wordt gebruikt om de rijrichting van de robot (RobotHoek) te bepalen.
Een andere constante, ODO_TICK_L_R_F, wordt gebruikt om verschillen in wielgrootte te corrigeren. Om te beginnen zetten we deze op 1.0, 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_F 1.0
#define ODO_HEADING_F 0.000168 // ODO_TICK_TO_METRIC_F / Wiel-afstand (in mm)
// Als de robot te weinig draait, moet dit getal lager worden
Stap 4: PWM Calibratie
Nu de aansturing van de motoren goed werkt en de encoders de juiste terugmelding geven, gaan we het verband meten tussen de PWM waarde en de motorsnelheid. Dit doen we door de motoren aan te sturen op 75% PWM en -25% PWM en vervolgens de draaisnelheid te meten. Als je voldoende ruimte hebt, kun je hiervoor de robot op de vloer zetten. Alternatief is om de wielen van de robot juist vrij te houden van de grond.
Het resultaat van de meting is afhankelijk van de voedingsspanning, dus als deze sterk wijzigt (bijvoorbeeld van een 3-cel lipo naar een 4-cel lipo), herhaal dan deze calibratie-stap.
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:
//-------------------------------------------------------------------------------- // Note: this calibration varies with MAIN_TAKT_INTERVAL and motor supply voltage. #define MOTOR_L_F_SPEED_GAIN_F 28.547691 #define MOTOR_L_F_OFFSET 12 #define MOTOR_L_R_SPEED_GAIN_F 28.564507 #define MOTOR_L_R_OFFSET -15 #define MOTOR_R_F_SPEED_GAIN_F 27.656639 #define MOTOR_R_F_OFFSET 18 #define MOTOR_R_R_SPEED_GAIN_F 27.738998 #define MOTOR_R_R_OFFSET -13 // Maxspeed ca 599 mm/s //--------------------------------------------------------------------------------
Neem dit blok over in Prj_RobotLib_conf.h.
Stap 5: Afstand (fijn)
Met een nauwkeurigheid van 20% of beter kunnen we de encoders gebruiken om de robot rechtuit te laten rijden. En... als de robot rechtuit rijdt, kunnen we de afgelegde weg nauwkeuriger meten met onze rolmaat. Ofwel, de fijne calibratie, in de volgende stappen:
- Zet de robot op vast punt met voldoende ruimte voor zich, bijvoorbeeld met de achterkant tegen een muur.
- PF8 - 'reset robot-positie'
- PF2 - '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 PF10 - '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 6: Hoek (fijn) en Afwijking wielgrootte
In de voorgaande stappen is de afstand gekalibreerd en is een aanvang gemaakt met het kalibreren van de rijrichting. In deze stap wordt de rijrichting calibratie verbeterd. Hierbij wordt ook gecorrigeerd voor een eventueel verschil in wielgrootte. We hebben hiervoor twee varianten. Met deze stappen wordt de calibratie van de robot afgerond.
UmbMark
De eerste variant die we kunnen gebruiken is UMBmark. Het is een bewezen methode waarmee zowel draaihoek als de afwijking in wielgrootte wordt gecorrigeerd.
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)
- Te ver rijden = positieve waarde, niet ver genoeg = negatieve waarde.
- 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.
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 B5 t/m B5 van de Bestand:Odo calibratie UmbMark v0.3.xlsx
- Zet de waarde van ODO_HEADING_F (uit Prj_RobotLib_conf.cpp) in cel B10.
- Zet de waarde van ODO_TICK_L_R_F (uit Prj_RobotLib_conf.cpp) in cel B17.
- Cel C10 bevat de nieuwe waarde voor ODO_HEADING_F, neem deze over in Prj_RobotLib_conf.cpp.
- Cel C17 bevat de nieuwe waarde voor ODO_TICK_L_R_F, neem deze over in Prj_RobotLib_conf.cpp.
Herhaal dit totdat de gewenste nauwkeurigheid is bereikt (zie ook verder).
Alternatief
Om een grote nauwkeurigheid te halen met UmbMark is een vierkant van 2x2 meter of groter aan te raden. Maar het is niet altijd mogelijk om zo'n vrije ruimte te vinden en zeker niet met een mooie vlakke vloer. In deze gevallen kunnen onderstaande stappen als alternatief worden gebruikt:
HoekDraaiing
- Zet de robot op de grond in een goed herkenbare startrichting. Het kan hierbij helpen om een laserpointer op de robot te monteren: de punt van de laserpointer op de wand maakt het uitrichten een stuk nauwkeuriger.
- Geef het commando 'um rotate 360 0'. De robot draait nu 360 graden.
- Draai de robot voorzichtig totdat deze weer in de startrichting staat (let op: de robot kan bij het draaien iets verplaatst zijn. Het gaat om de richting, niet om het exacte punt op de wand).
- Vraag de hoek van de robot op met het commando 'position'.
- Bereken de nieuwe waarde van ODO_HEADING_F met de formule:
New = ODO_HEADING_F * (1 + hoekfout / 360)
- Neem de nieuwe waarde over in Prj_RobotLib_conf.cpp.
- Herhaal de test. Indien de afwijking klein is, kan de robot meerdere rondjes worden gedraaid, bijvoorbeeld met 'um rotatae 3600 0'.
'WielGrootte
- Zet de robot op de grond in een goed herkenbare startrichting en met vrije ruimte voor zich.
- Laat de robot heen en weer rijden met het commando 'wscal'. Dit commando heeft twee parameters: de af te leggen weg en de snelheid. 'wscal 1500 300' laat de robot anderhalve meter vooruitrijden met een snelheid van 30 cm/s, dan omdraaien, terugrijden en weer in de startrichting draaien. De afgelegde weg is dus 3 meter (3000mm).
- Draai de robot voorzichtig totdat deze weer in de startrichting staat (let op: de robot kan bij het rijden iets verplaatst zijn. Het gaat om de richting, niet om het exacte punt op de wand).
- Vraag de hoek van de robot op met het commando 'position'.
- Bereken de correctiefactor C met de formule:
C = 1 + (wielafstand * hoekfout) / (57.33 * afgelegde_weg) waarbij wielafstand = afstand tussen de wielen in mm hoekfout in graden afgelegde_weg in mm
- Maak ODO_TICK_L_R_F gelijk aan ODO_TICK_L_R_F * C
- Herhaal de test. Indien de afwijking klein is, kun je de robot meerdere keren op en neer laten rijden zodat je de afwijking beter kunt bepalen.
Maximale nauwkeurigheid
Met bovenstaand stappenplan kan 'maximale' nauwkeurigheid worden gehaald. Maar wat is die maximale nauwkeurigheid, waar wordt deze door bepaald en hoe zinvol is het om hier het onderste uit de kan te halen?
De maximale nauwkeurigheid bij calibratie wordt bepaald door allerlei 'onbepaalde', niet systematische afwijkingen. Naarmate de fouten kleiner worden is het verstandig niet 1 meting te doen, maar meerdere. Rij bijvoorbeeld 3x het vierkantje linksom en 3x rechtsom. Als de verschillen tussen de metingen groter worden dan de gemiddelde fout, zal de calibratie niet veel verbeterd kunnen worden. Tenzij... je de bron van de 'onbepaalde' afwijkingen kunt aanpakken. Is de ondergrond mooi vlak, de robot stijf genoeg, zijn de wielen mooi rond, slingeren ze niet en is er geen slip bij het testen?
De genoemde bronnen van afwijking geven al aan dat de nauwkeurigheid bij praktisch gebruik van de robot minder zal zijn dan bij de kalibratie. Snelheidswisselingen tijdens de roborama wedstrijden geven, zeker samen met een hogere snelheid, een grotere kans op slip dan bij de rustige kalibratie. Ook een andere vloer zal de kalibratie beïnvloeden. Dus kijk vooral naar de nauwkeurigheid die nodig is en stop tijdig. Tenzij je natuurlijk meedoet aan Zolderkamer uitdagingen nummer 2.
Tunen regellussen
Het voorgaande beschrijft hoe we de motorsturing en odometrie calibreren. Daarnaast is het nodig dat ook de regellussen goed ingesteld staan. Dit is afhankelijke van de gebruikte motoren, encoders en de opbouw van de robot. Mogelijk is de standaard instelling goed, maar het kan ook nodig zijn dat deze wat bijgesteld moet worden om netjes te rijden. En in extreme gevallen kan de regeling zo verkeerd zijn, dat je de bovenstaande stappen niet kunt doorlopen.
PID controllers
De regellussen zijn gebaseerd op PID controllers. Het volgende menu geeft toegang tot de PID controllers:
SimBot> ? pid Pid commands pid list - list all PIDs (with output value) pid print <nr> - print all info of pid <nr> pid type <nr> <type> - set type for autotune pid debug <nr> <1/0> - set debug output on/off pid tunings <nr> <Kp> <Ki> <Kd> - set Kp Ki Kd (floats) pid leaky <nr> <Leaky> - Leaky integrator 0.0-1.0 (1 = no leak) pid limits <nr> <min><max> - set output limits pid auto <nr> <mode> - auto (1) or manual (0) mode pid reverse <nr> <rev> - reverse 0=direct, 1=reverse
Het command 'pid list' geeft een genummerd overzicht van de PID controllers.
SimBot> pid list PidList Pid 0 PID-MotorL (0) Pid 1 PID-MotorR (0) Pid 2 PID-SpeedHeading (0) Pid 3 PID-Linefollower (0) Done.
Het volgnummer wordt gebruikt om de PID te benaderen, bijvoorbeeld in met het commando 'pid print'.
SimBot> pid print 0 PID-MotorL Kp: 6.500000, Ki: 0.000000, Kd: 0.000000 (6.500000 0.000000 0.000000) Min: -4096, Out: 4096, Direction: Direct Mode: Automatic, Input: 0, Setpoint: 0, Out: 0, ClearI: 1, Leaky: 1.000000 TunerType: 'Ziegler-Nichols P' (3) InValue: 0, SpValue: 0, Out: 0, ITerm: 0.000000, DTerm: 0.000000
Op de eerste regel zie je de drie belangrijkste parameters: Kp, Ki en Kd. Als we straks aan de slag gaan met de autotuner, is het 'TunerType' relevant. Dit bepaalt hoe de regellus reageert doordat het de vertaling van de tuner resultaten (de versterking (Ku) en periodetijd (Pu)) te vertalen naar Kp, Ki en Kd. De beschikbare types kun je opvragen met:
PittMan> tune types Nr Name Mp Mi Md 0 Ziegler-Nichols PID 0.600000 0.500000 0.125000 1 Ziegler-Nichols PD 0.800000 0.000000 0.125000 2 Ziegler-Nichols PI 0.450000 0.833000 0.000000 3 Ziegler-Nichols P 0.500000 0.000000 0.000000 10 Pessen Integral Rule 0.700000 0.400000 0.050000 11 Some overshoot PID 0.330000 0.500000 0.333000 12 No overshoot PID 0.200000 0.500000 0.333000
De Motor-PIDs, die we zo gaan tunen, hebben deze type 3: een P-controller zoals beschreven door de uitvinders van deze tuning method, Ziegler en Nichols.
PID-MotorL en PID-MotorR
Deze twee PID controller vertalen snelheid naar PWM voor de beide motoren. Dit gebeurt in combinatie met 'feed forward' uit de motor-calibratie. Kleine verschillen tussen de motoren worden door deze calibratie afgevangen, zodat de PID controllers voor beide motoren meestal gelijk kan zijn.
De relevante default instellingen in Prj_RobotLib_conf.h:
//#define MOTOR_PID_TYPE PID_TYPE_ZN_P #define MOTOR_PID_KP (26.7) #define MOTOR_PID_KI (0.0) #define MOTOR_PID_KD (0.0)
Handmatig tunen (grof)
Deze regellus kan getest worden met 'um sr' (Speed Rotation). Als de robot schokkerig rijdt kan de versterking te hoog staan. Probeer in dit geval P lager te stellen. I en D kunnen op 0 gezet worden.
Autotune
Robotlib heeft een ingebouwde auto-tuner. Over auto-tuners wordt gezegd dat ze minder presteren dan een regeltechnicus. Als minder ervaren amateur vindt ik ze vaak erg nuttig, al is het alleen maar om een aardig startpunt te krijgen voor verdere optimalisatie.
De autotuner werkt door de aansturing wat te variëren en vervolgens te meten wat het effect is. Zorg daarvoor voldoende vrije ruimte op een - bij voorkeur - vlakke vloer. De instellen van een autotuner zijn afhankelijk van de omstandigheden (zie hier). Voor een aantal PID controllers van RobotLib zijn die instellingen voorgeprogrammeerd in de TunerHelper module.
De instellingen voor de PID-Motor* controllers laad je met 'tune helper 0':
PittMan> tune helper 0 200 1000 Setup AutoTune helper for 'PID-MotorL' (0), Speed 200 and max Distance 1000
De parameters zijn nu ook ingesteld. Tune op de snelheid die je het meest zult gebruiken, bijvoorbeeld 75% van het maximum. Als de opgegeven snelheid te hoog is, krijg je een waarschuwing, maar de parameters worden wel ingesteld. Je kunt het dus proberen.
Als de parameters ingesteld zijn, wordt het tunen gestart:
PittMan> tune start ResetRobotPosition Setup autotune. ========= Tuner =================== Autotuner pid: PID-MotorL, State: 0, StartDelay: 2500, AutoSp: 1 Setpoint: 0, NoiseBand: 131, OutStart: 0, OutputStep: 407 In -131 ... 131 -> out -407 ... 407 Last attempt - Succes: 0, Cycles: 0 Helper Nr: 0, Speed: 200, Distance: 1000 Input stats: Average: 0.000000, Variance: 0.000000, Stdev: 0.000000, N: 0, Min: 0.000000, Max: 0.000000 Output stats: Average: 0.000000, Variance: 0.000000, Stdev: 0.000000, N: 0, Min: 0.000000, Max: 0.000000 Commit / Preview error: no valid tuning parameters =================================== [37:I] MissionHandler(4): start mission MSM_AutoTuneSupervisor. PittMan> [37:I] Mission(4 at 611774, after 611774 ms) 'MSM_AutoTuneSupervisor' transition -1 -> 0 (4) [11:W] Start tuning - input: 1858, in_range: 1727/1989 out_range: -407/407 [11:E] Autotune results Ku: 1.353026, Pu: 0.300000 MSM_AutoTuneSupervisor: tune done [37:I] Mission(4 at 615424, after 3650 ms) 'MSM_AutoTuneSupervisor' transition 0 -> 10 (4) [37:I] Mission(4 at 616424, after 1000 ms) 'MSM_AutoTuneSupervisor' transition 10 -> 20 (4) [37:I] Mission(4 at 619724, after 3300 ms) 'MSM_AutoTuneSupervisor' transition 20 -> 30 (4) ========= Tuner =================== Autotuner pid: PID-MotorL, State: 0, StartDelay: 2500, AutoSp: 1 Setpoint: 1858, NoiseBand: 131, OutStart: 0, OutputStep: 407 In 1727 ... 1989 -> out -407 ... 407 Last attempt - Succes: 1, Cycles: 3 Helper Nr: 0, Speed: 200, Distance: 1000 Input stats: Average: 1941.149902, Variance: 69190.976562, Stdev: 263.041777, N: 20, Min: 1546.000000, Max: 2312.000000 Output stats: Average: 40.699997, Variance: 172623.703125, Stdev: 415.480088, N: 20, Min: -407.000000, Max: 407.000000 Cycle 0: Stamp: 615024, Min: 1573, Max: 2312 Cycle -1: Stamp: 614724, Min: 1546, Max: 2298 Cycle -2: Stamp: 614424, Min: 1550, Max: 2238 Preview for pid 'PID-MotorL' and pid 'PID-MotorR' Input from autotune: Ku: 1.353026, Pu: 0.300000 Autotune Type: 'Ziegler-Nichols P' (3) params: Kp: 0.676513, Ki: 0.000000, Kd: 0.000000 =================================== [37:I] Mission(4) 'MSM_AutoTuneSupervisor' done
Het tuning process geeft veel output. Het belangrijkste is de melding ‘tune done’ en ‘Succes: 1’. Dit wordt ook duidelijk doordat de robot achteruit terug rijdt naar de start. Als de tuning mislukt, draait de robot aan het einde om, rijdt dan vooruit terug naar de start en draait weer vooruit. Veel voorkomende oorzaken waardoor tuning mislukt:
- Snelheid te hoog, kies voor 33 - 66% van het maximum.
- Te weinig ruimte: bij voldoende ruimte probeert de tuner 10 slingers en stopt als er 3 op rij (nagenoeg) gelijk zijn.
- Instellingen van bijvoorbeeld de noiseband. Zie hier voor meer informatie.
Bij ‘succes’ staat aan het einde van de output de ‘preview’, de waarden die de auto tuner voorstelt. Deze kun je toepassen (activeren) met 'tune commit':
PittMan> tune commit Commit for pid 'PID-MotorL' and pid 'PID-MotorR' Input from autotune: Ku: 1.353026, Pu: 0.300000 Autotune Type: 'Ziegler-Nichols P' (3) params: Kp: 0.676513, Ki: 0.000000, Kd: 0.000000 PittMan>
Let op: tune commit maakt de waarden niet permanent, waardoor de instelling na reset weer weg zijn. Om de instellingen permanent te maken zijn er twee opties:
- Sla de configuratie op in eeprom met behulp van de 'non-volatile' module (zie volgende tune stap - PID-SpeedHeading - als voorbeeld. Let op dat beide motoren tegelijk getuned worden, maar dat de configuratie per motor opgeslagen wordt.).
- Neem de instellingen over als defaults in prj_robotlib_conf.h aan te passen:
// PID-Motor L/R constants #define MOTOR_PID_KP (0.68) #define MOTOR_PID_KI (0.0) #define MOTOR_PID_KD (0.0)
PID-SpeedHeading
Deze PID controller vertaalt rijrichting naar snelheid voor de linkse en rechtse motor, zodat de robot in de juiste richting rijdt. Het wordt gebruikt in UmSpeedHeading (rij met een bepaalde snelheid in een gevraagde richting). En omdat UmSpeedHeading weer door vele andere UniversalMovements wordt gebruikt (zoals het veelgebruikte UmXY), heeft deze controller ook hierop invloed.
De relevante default instellingen in Prj_RobotLib_conf.h:
// UmSpeedHeading constants //#define SPEED_HEADING_PID_TYPE PID_TYPE_NO_OVERSHOOT_PID #define SPEED_HEADING_PID_KP 0.0134 #define SPEED_HEADING_PID_KI 0.0463 #define SPEED_HEADING_PID_KD 0.000260 #define SPEED_HEADING_PID_CLIP 3000
Deze regeling is bedoeld om de robot, die ongeveer op koers zit, op koers te houden. Het is dus het beste om een robot eerst in de goede richting te draaien. Maar dit zal niet altijd gebeuren en om de robot dan toch nog redelijk te laten bewegen, hebben we de parameter SPEED_HEADING_PID_CLIP. Hiermee wordt de grootte van de correctie beperkt.
Handmatig tunen (grof)
Deze regellus kan getest worden met 'um sh' (Speed Heading). Als de robot slingert bij het rijden van een rechte lijn, is PID_Kp waarschijnlijk te hoog. Bij lage waarde van PID_Kp zal de robot maar langzaam bijsturen als de rijrichting niet goed is.
SPEED_HEADING_PID_CLIP pas lager instellen nadat PID_Kp is ingesteld. Geef de robot opdracht om een andere kant op te rijden. Als de robot zeer snel naar de gevraagde rijrichting draait (en mogelijk ook te ver draait ('overshoot')), staat deze waarde te hoog. Bij een te lage waarde van PID_OUT_CLIP rijdt de robot in een (te) wijde boog naar de gewenste richting.
= Autotune
Nadat de motor-PIDs zijn gecalibreerd, kan de PID voor de rijrichting gecalibreerd worden met tune helper 1.
PittMan> tune helper 1 200 1000 Setup AutoTune helper for 'PID-SpeedHeading' (1), Speed 200 and max Distance 1000 PittMan> tune start
Hieronder een voorbeeld waarbij het tunen mislukt:
ResetRobotPosition
Setup autotune.
========= Tuner ===================
Autotuner pid: PID-SpeedHeading, State: 0, StartDelay: 1000, AutoSp: 1
Setpoint: 1910, NoiseBand: 2000, OutStart: 0, OutputStep: 150
In -90 ... 3910 -> out -150 ... 150
Last attempt - Succes: 0, Cycles: 3
Helper Nr: 1, Speed: 200, Distance: 1000
Input stats: Average: 1927.750000, Variance: 72563.039062, Stdev: 269.375276, N: 20, Min: 1504.000000, Max: 2283.000000
Output stats: Average: 40.699997, Variance: 172623.703125, Stdev: 415.480088, N: 20, Min: -407.000000, Max: 407.000000
Cycle 0: Stamp: 39674, Min: 1549, Max: 2279
Cycle -1: Stamp: 39374, Min: 1534, Max: 2252
Cycle -2: Stamp: 39074, Min: 1504, Max: 2283
Commit / Preview error: no valid tuning parameters
===================================
[37:I] MissionHandler(4): start mission MSM_AutoTuneSupervisor.
PittMan> [37:I] Mission(4 at 110874, after 66400 ms) 'MSM_AutoTuneSupervisor' transition -1 -> 0 (4)
[11:W] Start tuning - input: -1662, in_range: -3662/338 out_range: -150/150
MSM_AutoTuneSupervisor: tune canceled
[37:I] Mission(4 at 116874, after 6000 ms) 'MSM_AutoTuneSupervisor' transition 0 -> 100 (4)
[37:I] Mission(4 at 117274, after 400 ms) 'MSM_AutoTuneSupervisor' transition 100 -> 110 (4)
UmMotorControlDump State: 2, PwmSp 0 / 0
SpeedSp: 0, RotateSp: 0, Mode: 1
MaxRR: 3.140000, MaxRS: 31.400000, CurrentRR: 0.689178
SpeedCurve Dist: 0, Speed: 3, Acc: 75, Jerk: 1500
MaxJerK 1500, MaxAcc: 500, SaveSpeed: 0, SaveAcc: 0
[37:I] Mission(4 at 122974, after 5700 ms) 'MSM_AutoTuneSupervisor' transition 110 -> 120 (4)
UmMotorControlDump State: 2, PwmSp 0 / 0
SpeedSp: 0, RotateSp: 0, Mode: 1
MaxRR: 3.140000, MaxRS: 31.400000, CurrentRR: 0.473442
SpeedCurve Dist: 34, Speed: 0, Acc: 0, Jerk: 1500
MaxJerK 1500, MaxAcc: 500, SaveSpeed: 111, SaveAcc: 100
[37:I] Mission(4 at 124574, after 1600 ms) 'MSM_AutoTuneSupervisor' transition 120 -> 130 (4)
========= Tuner ===================
Autotuner pid: PID-SpeedHeading, State: 0, StartDelay: 1000, AutoSp: 1
Setpoint: -1662, NoiseBand: 2000, OutStart: 0, OutputStep: 150
In -3662 ... 338 -> out -150 ... 150
Last attempt - Succes: 0, Cycles: 3
Helper Nr: 1, Speed: 200, Distance: 1000
Input stats: Average: -1841.729248, Variance: 4395512.500000, Stdev: 2096.547758, N: 96, Min: -11087.000000, Max: 1402.000000
Output stats: Average: 31.250004, Variance: 21749.994141, Stdev: 147.478792, N: 96, Min: -150.000000, Max: 150.000000
Cycle 0: Stamp: 114824, Min: -11087, Max: 724
Cycle -1: Stamp: 113224, Min: -4250, Max: 893
Cycle -2: Stamp: 112174, Min: -4408, Max: 1402
Commit / Preview error: no valid tuning parameters
===================================
[37:I] Mission(4) 'MSM_AutoTuneSupervisor' done
In dit geval zien we de melding ‘tune canceled’ en ‘Succes: 0’. Dit betekent dat de tune poging mislukt is en dit maakt de overige data relevant. Bij het tunen ‘slingert’ de tuner wat op en neer (waardoor de robot vaak ook slingert) en doet daarbij metingen. Tune is gelukt als er 3 cycles achter elkaar zijn die ongeveer dezelfde amplitude en periodetijd. Vaak zijn 4-6 cycles nodig voor een goede tune, maar is er onvoldoende ruimte om dit uit te voeren. Dit is hierboven het geval. Er zijn precies drie cycles gedaan, maar het verschil tussen min en max van de eerste is duidelijk anders dan die van de volgenden. Als de tuner na een groot aantal cycles nog geen succes heeft, betekent dit dat resultaat niet stabiel is. Meestal als gevolg van de gebruikte parameters . Ook kan het zijn dat tuner geen cycles ziet. Ook dat is meestal een gevolg van de gebruikte parameters…
In dit geval is onvoldoende afstand (tijd) het probleem. Een nieuwe poging over anderhalve meter is wel succesvol:
PittMan> tune helper 1 200 1500 Setup AutoTune helper for 'PID-SpeedHeading' (1), Speed 200 and max Distance 1500 PittMan> tune start ResetRobotPosition Setup autotune. ========= Tuner =================== Autotuner pid: PID-SpeedHeading, State: 0, StartDelay: 1000, AutoSp: 1 Setpoint: -1603, NoiseBand: 2000, OutStart: 0, OutputStep: 150 In -3603 ... 397 -> out -150 ... 150 Last attempt - Succes: 0, Cycles: 3 Helper Nr: 1, Speed: 200, Distance: 1500 Input stats: Average: -2295.308838, Variance: 4987016.500000, Stdev: 2233.162892, N: 94, Min: -12110.000000, Max: 1662.000000 Output stats: Average: -15.957445, Variance: 22484.560547, Stdev: 149.948526, N: 94, Min: -150.000000, Max: 150.000000 Cycle 0: Stamp: 142524, Min: -12110, Max: 684 Cycle -1: Stamp: 141274, Min: -4211, Max: 889 Cycle -2: Stamp: 140274, Min: -4301, Max: 1662 Commit / Preview error: no valid tuning parameters =================================== [37:I] MissionHandler(4): start mission MSM_AutoTuneSupervisor. PittMan> [37:I] Mission(4 at 163624, after 10800 ms) 'MSM_AutoTuneSupervisor' transition -1 -> 0 (4) [11:W] Start tuning - input: -573, in_range: -2573/1427 out_range: -150/150 [11:E] Autotune results Ku: 0.053483, Pu: 0.800000 MSM_AutoTuneSupervisor: tune done [37:I] Mission(4 at 168174, after 4550 ms) 'MSM_AutoTuneSupervisor' transition 0 -> 10 (4) [37:I] Mission(4 at 169174, after 1000 ms) 'MSM_AutoTuneSupervisor' transition 10 -> 20 (4) [37:I] Mission(4 at 173074, after 3900 ms) 'MSM_AutoTuneSupervisor' transition 20 -> 30 (4) ========= Tuner =================== Autotuner pid: PID-SpeedHeading, State: 0, StartDelay: 1000, AutoSp: 1 Setpoint: -573, NoiseBand: 2000, OutStart: 0, OutputStep: 150 In -2573 ... 1427 -> out -150 ... 150 Last attempt - Succes: 1, Cycles: 4 Helper Nr: 1, Speed: 200, Distance: 1500 Input stats: Average: -494.500031, Variance: 3807220.500000, Stdev: 1951.210009, N: 68, Min: -4049.000000, Max: 3093.000000 Output stats: Average: -8.823530, Variance: 22756.804688, Stdev: 150.853587, N: 68, Min: -150.000000, Max: 150.000000 Cycle 0: Stamp: 167074, Min: -3619, Max: 2628 Cycle -1: Stamp: 166224, Min: -3888, Max: 2667 Cycle -2: Stamp: 165474, Min: -4049, Max: 3093 Preview for pid 'PID-SpeedHeading' Input from autotune: Ku: 0.053483, Pu: 0.800000 Autotune Type: 'No overshoot PID' (12) params: Kp: 0.010697, Ki: 0.026741, Kd: 0.002850 ===================================
De voorgestelde parameters kunnen opgeslagen worden in eeprom met behulp van de 'non-volatile' module of als defaults opnemen in prj_robotlib_conf.h. Opslaan met behulp van non-volatile (nv) gaat als volgt:
- vraag de non-volatile objecten op met 'nv list'
PlankTrick> nv list NvList Nv 0 LineSensorQTR8A Nv 1 PID-MotorL Nv 2 PID-MotorR Nv 3 PID-SpeedHeading Nv 4 PID-Linefollower Done.
We willen de configuratie van nummer 3 - PID-SPeedHeading - opslaan.
PlankTrick> nv save 3 [61:I] NvStore.save PID-SpeedHeading [61:I] NvCfg.ValidData - save done. PlankTrick> IntervalGuard: MainTaktInterval too long (Duration: 50, Setpoint: 20 ms)
De laatste melding geeft aan dat het opslaan zo lang duurde dat de timing van de robot in de war raakt. Geen probleem als de robot stil staat en we weten wat de oorzaak is (toegang tot het filesysteem via i2c, wat relatief traag is).
Met het commando 'file list' krijgen we een overzicht van de bestanden op het filesysteem:
file list Mfs list file: 'PID-SpeedHeading' (c) - 12 bytes file: 'PID-MotorL' (c) - 12 bytes file: 'PID-MotorR' (c) - 12 bytes Done. PlankTrick> IntervalGuard: MainTaktInterval too long (Duration: 50, Setpoint: 20 ms)
Het bestand 'PID-SpeedHeading' bevat de configuratiegegevens van de PID tuner. De andere twee bestanden bevatten de eerder opgeslagen configuratie van de motoren L en R. Deze worden automatisch geladen bij het opstarten van de robot. Als het bestand niet aanwezig is, worden de defaults gebruikt die met #define opgegeven kunnen worden.
PID-Linefollower
Deze controller wordt gebruikt om een lijn te volgen. De tune helper heeft geen standaard instellingen voor deze controller.
Draaien
Hierboven is aangegeven dat het de voorkeur heeft om de robot in de goede richting te zetten, voordat deze in de richting gaat rijden. Dit kan met UmRotate. Het gedrag van UmRotate wordt bepaald door de volgende parameters:
// Rotate constantes #define ROTATE_CLIP (10*256) #define ROTATE_P_GAIN (0.08) #define ROTATE_D_GAIN (0.05)
De parameter ROTATE_CLIP heeft een vergelijkbare functie als de PID_OUT_CLIP voor de rijrichting, namelijk de maximale correctie (draaisnelheid) beperken. Voor het draaien wordt een PD-regeling gebruikt, vandaar dat deze regeling naast een P (Kp) parameter, ook een D parameter heeft.
Tunen:
ROTATE_P_GAIN bepaalt de draaisnelheid en moet groot genoeg zijn om ook bij kleine stukjes draaien de robot in beweging te krijgen. ROTATE_D_GAIN wordt gebruikt om, met name na een wat langere draai, de robot op het juiste punt te laten stoppen (niet teveel overshoot).
ROTATE_CLIP wordt weer als laatste ingesteld en beperkt de maximale draaisnelheid.
Versnellen en vertragen
De meeste UniversalMover routines zorgen ervoor dat de snelheid van de robot niet te snel verandert. Rustig optrekken en niet bovenop de rem als je wilt stoppen. Hoe rustig of vlot je robot versnelt en vertraagt, kun je instellen met:
#define MAX_SLOPE 50 // in mm/sec/MAIN_TAKT_INTERVAL
Een hogere waarde betekent vlotter rijden, dus snel optrekken en laat remmen. Goed dus als je een snelle tijd weg wilt zetten. Maar... wel meer kans op slip, waardoor de robotpositie minder nauwkeurig is.