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. En daarna de 'hogere' regellussen. 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 PrjMotorInit(); // initialiseer alle motor- en encoder-gerelateerde zaken, zoals GPIO, PWM en counters. void PrjMotorPwm(int PwmL, int PwmR); // Stel motoren in op gegeven PWM waarde; range van 4095 (vol gas vooruit) tot -4096 (vol gas achteruit) void PrjEncoderRead(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.
PrjMotorPwm()
PFKeyMotorCalibrate1 heeft functie-toetsen waarmee PwmL en PwmR verhoogd en verlaagd kunnen worden, onafhankelijk van elkaar. Je kunt een lijst met geladen PFKeyHandlers opvragen met 'pf list' en een handler selecteren met 'pf set n'.
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:
- Later gaan we de draairichting van de motoren nog instellen en de linkse en rechtse motor omwisselen als dat nodig is. Dus dat is nu nog niet van belang.
- 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.
PrjEncoderRead()
Robotlib leest de encoders uit met een aanroep van de routine PrjEncoderRead().
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 PrjEncoderRead() 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.
Het teken van de encoder (vooruit of achteruit) gaan we later instellen. Voor nu is het alleen relevant dat de linkse motor gekoppeld is aan de linkse encoder en de rechtse motor aan de rechtse encoder.
Stap 2 - Links/rechts en draairichting
Het commando 'position defines' geeft (onder meer) informatie over de motor-configuratie:
#define MOTOR_LR_SWAP 0 #define MOTORL_PWM_REVERSE 1 #define MOTORL_ENCODER_REVERSE 1 #define MOTORR_PWM_REVERSE 1 #define MOTORR_ENCODER_REVERSE 1
Stappen:
- Geef het commando 'um pwm 1000 0' om de (vermeende) linkse motor vooruit te laten draaien.
- Zet MOTOR_LR_SWAP op true of false (met 'position mswap'), zodat het de linkse motor is die draait.
- Zet MOTORL_PWM_REVERSE op true of false (met 'position mlpwm'), zodat het de linkse motor vooruit draait.
- Geef het commando 'um pwm 0 1000' om de rechtse motor vooruit te laten draaien.
- Zet MOTORR_PWM_REVERSE op true of false (met 'position mrpwm'), zodat het de rechtse motor vooruit draait.
- Maak de instellingen permanent met 'position save'.
En vervolgens
- Sluit de robot aan op accu die 50% - 90% is geladen of een vergelijkbare voeding.
- Geef commando 'motorcal'.
Dit commando controleert de werking van de motoren. De melding 'Motor setup issues, fix code or hardware.' geeft aan dat er iets niet goed is gegaan in stap 1a.
Indien nodig worden de ENCODER_REVERSE waarden aangepast. In dat geval wordt gevraagd deze instelling permanent te maken. Dit kan met het commando 'position save'. Daarna kan 'motorcal' worden herhaald.
Als alles goed gaat wordt dit aan het einde geprint:
//-------------------------------------------------------------------------------- // Note: this calibration varies with MAIN_TAKT_INTERVAL and motor supply voltage. #define MOTOR_L_F_SPEED_GAIN_F 14.759370 #define MOTOR_L_F_OFFSET 145 #define MOTOR_L_R_SPEED_GAIN_F 14.237679 #define MOTOR_L_R_OFFSET -156 #define MOTOR_R_F_SPEED_GAIN_F 13.791245 #define MOTOR_R_F_OFFSET 114 #define MOTOR_R_R_SPEED_GAIN_F 14.282026 #define MOTOR_R_R_OFFSET -101 // Maxspeed ca 729 mm/s //--------------------------------------------------------------------------------
Neem deze waarden over in 'Prj_RobotLib_conf.h'.
Stap 3: 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 'position reset'
- Draai beide wielen (zoveel mogelijk tegelijk) 1 omwenteling vooruit. Als het lukt zonder teveel wielslip, de robot iets verplaatsen zodat je de afgelegde weg kunt meten.
- Geef het commando 'position print'
T-Master> position print RobotPosition X: 294, Y: 0, Hoek: 0 (192)
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.
In dit geval was de gemeten afstand (in de x-richting) 190mm. Verwerk dit met het commando 'position cald':
T-Master> position cald 190
Calibrate Distance - 294 -> 190 0.100000 -> 0.064626
Position.Commit()
Position.Dump()
Default - TickToDist: 0.100000, TickToHeading: 0.000400, TickLR: 1.000000
Current - TickToDist: 0.064626, TickToHeading: 0.000400, TickLR: 1.000000
TickToDist: 6.462585e-02, TickToHeading: 4.000000e-04, TickLR: 1.000000e+00
Scale: 1.000000
Derived: T2D: 0.064626 / 0.064626, T2H: 0.000400 / 0.000400
Dit commando berekent de correctie-factor en slaat dit op. Herhaal voorgaande stappen tot de calibratie binnen 20% nauwkeurig is.
De default (meest permanente) waarde van ODO_TICK_TO_DISTANCE wordt bepaald n prj_robotlib_conf.h. Met het commando 'position save' wordt de waarde opgeslagen in NV geheugen (eeprom). Na reset wordt deze waarde gelezen.
Stap 4: Hoek
Meet de afstand tussen de twee wielen (spoorbreedte) en geef dit in, bijvoorbeeld 160 mm:
position setwidth 160
Robotlib berekent TickToHeading op basis van TickToDistance en de ingegeven waarde. Maak dit permanent met 'position save'.
We kunnen nu TickToHeading nauwkeuriger bepalen met:
- Reset de robot-positie ('position reset')
- Laat de robot 360 graden draaien ('um rotate 360 0 100')
- Corrigeer de richting van de robot voorzichtig tot 360 'echte' graden. Doe dit zonder dat de wielen slippen. Bijvoorbeeld door met beide duimen bovenop de wielen te drukken en ze zo wat te verdraaien.
- Geef het command 'postion calt2h 1', waarbij 1 staat voor 1 * 360 graden.
- Geef command 'position save' om de waarde permanent te maken.
Herhaal bovenstaande eventueel met meer rondjes, bijvoorbeeld 5 rondjes == 1800 graden:
um rotate 1800 0 100
en na correctie van de richting calibreren
postion calh 5 position save
Tot nu toe zijn de instelling opgeslagen in de eeprom. De instelling worden echt permanent door ze op te nemen in Prj_RobotLib_conf.h. Alle instellingen kun je opvragen met 'position defines', waarbij ODO_TICK_TO_HEADING staat voor TickToHeading.
Bonkie> position defines //----------------------------------------------------------------------------- // motor/encoder parameters // // ODO_TICK_TO_DISTANCE - distance per tick, in mm/tick // ODO_TICK_CORRECT_LR - correct for wheel size difference // ODO_TICK_TO_HEADING - translates (calibrates) from raw odo ticks to heading, // larger => more degrees per ticks. If the robot turns // too much, this number needs to be larger. // #define ODO_TICK_TO_DISTANCE 0.136216 #define ODO_TICK_CORRECT_LR 1.000000 #define ODO_TICK_TO_HEADING 0.000851 #define MOTOR_LR_SWAP 0 #define MOTORL_PWM_REVERSE 0 #define MOTORL_ENCODER_REVERSE 1 #define MOTORR_PWM_REVERSE 0 #define MOTORR_ENCODER_REVERSE 1 //-----------------------------------------------------------------------------
Neem deze waarden over.
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).
- meet de gereden afstand met een ROLMAAT
- Geef de gemeten afstand in met het command 'position cald n', waarbij n de gemeten afstand in mm is.
- Op het scherm wordt (o.a.) de nieuwe waarde van ODO_TICK_TO_DISTANCE getoond. Deze wordt ook meteen geactiveerd (tot aan de volgende processor reset).
- De default (meest permanente) waarde van ODO_TICK_TO_DISTANCE wordt bepaald n prj_robotlib_conf.h.
- Met het commando 'position save' wordt de waarde opgeslagen in NV geheugen (eeprom). Na reset wordt deze waarde gelezen.
De afstandcalibratie is nu gereed.
Stap 6: Afwijking wielgrootte
In de voorgaande stappen is de afstand gekalibreerd en is een aanvang gemaakt met het kalibreren van de rijrichting. Hiermee kan een behoorlijke nauwkeurigheid worden bereikt In deze laatste, optionele stap wordt de rijrichting calibratie verder verbeterd. Hierbij wordt ook gecorrigeerd voor systematische fouten en 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_TICK_TO_HEADING (uit Prj_RobotLib_conf.cpp) in cel B10.
- Zet de waarde van ODO_TICK_CORRECT_LR (uit Prj_RobotLib_conf.cpp) in cel B17.
- Cel C10 bevat de nieuwe waarde voor ODO_TICK_TO_HEADING, neem deze over in Prj_RobotLib_conf.cpp.
- Cel C17 bevat de nieuwe waarde voor ODO_TICK_CORRECT_LR, 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.
- zet de robot positie op 0 met het commando 'position reset'
- Geef het commando 'um rotate 360 0 200'. De robot draait nu 360 graden (met een snelheid van 200 graden/sec).
- Draai de robot, als deze is gestopt, 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).
- geef het commando 'position calh 1' waarbij 1 staat voor 1 rondje van 360 graden. Dit commando werkt de waarde TickToHeading bij:
T-Master> position calh 1
Calibrate Heading - 0.836014 / 1 ; 0.000400 -> 0.000453
Position.Commit()
Position.Dump()
Default - TickToDist: 0.100000, TickToHeading: 0.000400, TickLR: 1.000000
Current - TickToDist: 0.100000, TickToHeading: 0.000453, TickLR: 1.000000
TickToDist: 1.000000e-01, TickToHeading: 4.532223e-04, TickLR: 1.000000e+00
Scale: 1.000000
Derived: T2D: 0.100000 / 0.100000, T2H: 0.000453 / 0.000453
- Herhaal de test. Indien de afwijking klein is, kan de robot meerdere rondjes worden gedraaid, bijvoorbeeld 10 rondjes met 'um rotatae 3600 0 300'. Het correctie commando wordt dan 'position calh 10'.
- Neem de nieuwe waarde van TickToHeading over in Prj_RobotLib_conf.cpp:
#define ODO_TICK_TO_HEADING 0.000453
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_CORRECT_LR gelijk aan ODO_TICK_CORRECT_LR * 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.
Epiloog: 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. De juiste instellingen zijn 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.
Stap 1 : 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. Het is dus belangrijk eerst de motor-calibratie uit te voeren, voordat je deze stap doet.
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 vind ik ze soms 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)
Samengevat (met PID 0 en PID 1 als PID-L en PID_R):
tune helper 0 200 3000 tune start tune commit pid save 0 pid save 1
Stap 2: 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.
Samengevat (met PID 2 als PID-SpeedHeading):
tune helper 1 300 3000 tune start tune commit pid save 2
Stap 3: PID-Linefollower
Deze controller wordt gebruikt om een lijn te volgen. De tune helper heeft geen standaard instellingen voor deze controller.
Stap 4: 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 SpeedCurveC1. Deze variabele is toegankelijk via de registery.
Opvragen:
reg list
output:
--- All Registry Variables -------------------- (i) Xpos = 1 (i) Ypos = 0 (i) Angle = 5 (f) SpeedCurveC1 = 0.003000 (i) Speed = 300 (i) Distance = 1000 ----------------------------------------------- PurpleBot>
Variabele SpeedCurveC1 is dus een float met waarde 0.003.
Je kunt dit aanpassen met:
reg set SpeedCurveC1 0.01
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. De waarde van SpeedCurveC1 kun je semi-permanent maken door deze op te slaan op het filesysteem met:
reg save SpeedCurveC1