Problem mit ACS und Linienverfolgung

Hallo,

ich hoffe mir kann jemand helfen,
soll für dir Schule einen RP6 programmieren,
habe mir zur Aufgabe gemacht den RP6 eine Linie verfolgen zu lassen und wenn ein Hindernis im Weg steht soll er es mittels ACS umfahren.

Nun mein Problem,
ich habe zu Test zwecken ein einfaches Linienverfolgungsprogramm das auf eine Kollision reagieren soll, leider tut es das nicht und ich weiss nicht warum.

Ich benötige dringend einen Anstoss in die richtige Richtung ich komm einfach nicht weiter.

Wäre super wenn ihr mir helfen könntet.