Kø system

På grunn av at controlleren kan bevege seg mye raskere en UR-roboten må vi innføre en måte slik at roboten kan følge etter bevegelsene og ikke hoppe rett til slutt posisjon. 

Når kalibreringssignalet xCalibrate er aktivert og ikke var aktivert i forrige syklus (NOT xCalibrate0), nullstilles tellerne iCounter_ og iCounterRobot som holder styr på posisjonsdataene og deres behandling. Dette sikrer at systemet starter friskt hver gang det kalibreres.

Feilsjekk og Dataoppbevaring:
Systemet kontinuerlig sjekker den nåværende joint-posisjonen (arJointState) med den forrige posisjonen som ble lagret(arPrevInputArray). Forskjellen mellom disse lagres i arErrorArray. Dersom denne feilen overskrider en akseptabel grense (rAcceptedError), indikeres dette ved at xKeepArray settes til TRUE. Dette skjer i en løkke som behandler hver joint individuelt. Denne grensens størrelse vil endre å hvor forskjellig neste posisjon må være fra forrige for at den skal være verdt og lagre. Standard satt til 1 grad.

Købehandling og Posisjonsoppdatering:
Når xKeepArray er sann, indikerer det at den nåværende posisjonen har en stor endring fra den forrige og skal lagres i køen aarDesiredJointsAngleList. Denne oppdaterte posisjonen settes også som den nye (arPrevInputArray) for å sammenlignes med fremtidige bevegelser. iCounter_ inkrementeres for å håndtere neste inputposisjon i køen.

Feedback og Deviasjonskontroll:
Kontrolleren mottar feedback fra roboten gjennom arUR5eFeedback, og sammenligner denne med de ønskede posisjonene lagret i køen for å beregne avvik (arJointAnglesDeviation). Systemet sjekker om avviket ligger innenfor et tolerert område (rAcceptedDeviation). Hvis roboten anses å være "nærme nok" (xRobotCloseEnough), oppdateres iCounterRobot for å flytte til neste posisjon i køen.

Resultathåndtering og Loop-kontroll:
Hvis roboten er nær nok den ønskede posisjonen, oppdateres måljointstaten (arJointstateTarget) til neste posisjon i køen. Hvis ikke, holdes den nåværende målposisjonen. Dette sikrer kontinuerlig oppdatering og justering basert på robotens faktiske posisjon sammenlignet med den ønskede banen.