LPD update:

 - doet geen motion, dus NOLOCKGROUP toegevoegd
 - gebruiken nu echte encoders, dus alles in mm: thresholds aangepast
 - meenemerpakketten hebben nu ook een id, dus geef die een geldige waarde

getest op R1, werkt.

weglegger y coord is relatief tov robot die meenemerpakket krijgt, dus
daarom 0 in de LPD.
Enkele debug registers toegevoegd.
Initialiseer ook id van meenemer pkt in reset.
Debug regs voor drop conditions.
Purge de ISR handler ook nog in init en cleanup.
Wegleg SM update:

 - SM kan nu meerdere (maar max 5) states in één iteratie van de main loop
   doorlopen: verkleint tijd tussen MOVE TO instructies: meer kans op
   smooth motion interpolation
 - meenemer kandidaat blijft in lijst totdat pepper gedropped is óf OOR:
   y coord wordt dan tot na drop bijgewerkt, ipv tot aan find_meen state
 - encoder van afvoer wordt nu gebruikt + snelheid afgeleid ipv gesimuleerd
 - thresholds voor 'drop to DIP' en 'drop pepper' states beter bepaald
 - eerste versie 'time of flight' estimator geimplementeerd mbv 'spider'
   TPP en lineaire schatter
 - vaste PUP_X wordt nu gebruikt ivm testen zonder aanv band mgr
 - halfway point is nu 3cm achter én boven PUP (als test)
 - IF clauses in find_meen herschreven ivm gespiegelde Y-as (robot tov
   afv band)
 - simpele 'cycle time' berekening toegevoegd (update in SM_DIP_ESCAPE)
 - skip S_HOVER_AANV als er een pepper binnengekomen is: ga dan naar
   S_WAIT_PEP
Update Z coordinaten van motion points

Robots 1 en 2 zijn 30 mm hoger op de sokkel gekomen. Gaat uiteindelijk ook
voor robot 3 gelden.