# Team Drive 2021
>:black_nib: Autoren: @daniel4937 @syldeva @timonnotter
>
>:books: Kurs: 6.491 | RoboCup2021
>
>:calendar: Datum: 06.05.2021
>
>:checkered_flag: Zielsetzung: PIDDrive -> A2B ohne Laser & Gripperchallenge
>

## Journal
| Datum | Was haben wir gemacht? | Was sind die weiteren Schritte? |
| -------- | -------- | -------- |
| 06.05.2021| Neuer Ansatz: Es werden zwei Varianten verfolgt, die eine stellt den DriveOnly "A2B" dar welche den Robi von A nach B fahren lässt. Nötige Methoden sowie Parameter gefunden. Die andere Variante ist mit A-Star, diese wird später berücksichtigt.| MQTT Topic ***FINAL*** testen, könnte schon viele Probleme lösen. Vermutlich wurde bisher nur über ***GOTOTARGET*** und nicht mit FINAL gearbeitet.|
| 07.05.2021|A2B Drive verbessert in dem wir CalcSpeed und Toleranz angepasst haben --> Kann Robi in FINAL überhaupt rückwerts fahren??|A2B Rückwerts funktioniert nicht, wie wollen wir dies gewährleisten?|
| 16.05.2021|Doku erweitert (Einführung, Ziele, Erkentnisse, Vorgehensweise)|Immernoch A2B sowie Doku parallel zur Praxis ergänzen|
| 07.05.2021|....|.....|
| 07.05.2021|....|.....|
| 07.05.2021|....|.....|
| 09.06.2021|Mit Node-Red Approach gestartet was nicht schlecht funktioniert|Gripper-Challenge automatisieren, wollen wir eine eigene Klasse machen mit einem eigenen Argument?|

## Einführung

Prinzipiell funktioniert das Drive "nicht schlecht". Wie schon erwähnt ist dieses Gebiet eines der komplexeren weswegen wir uns nach langem Einarbeiten auf etwas spezifisches fokussieren mussten. Anfangs versuchten wir uns einen Überblick zu machen wie die Klassen zusammen arbeiten und wo sie überhaupt gebraucht werden und wo nicht. Für diesen Schritt haben uns die UML-Diagramme sehr geholfen. Uns ist aufgefallen das viele Methoden deprecated oder sogar auskommentiert sind. Es ist relativ schwierig sich einen Überlick zu schaffen solange man nicht weiss ob jetzt diese Methoden doch gebraucht werden oder nicht.

### Vorgehensweise
In einer ersten Phase haben wir uns an den Code gemacht und diesen Versucht zu verstehen. Bei so einem komplexen Gebiet ist es jedoch fast unmöglich die ganze Funktion zu verstehen. Beim weiteren Vorgehen haben wir uns entschieden den Code zu säubern beziehungsweise Methoden die nicht gebraucht werden, zu entfernen. Auch dies war nicht besonders hilfreich weswegen wir uns schlussendlich auf das OnlyDrive konzentriert haben welches scheinbar seit längerem nicht richtig funktioniert.
Das OnlyDrive konnten wir verbesseren in dem wir die Toleranz verändert haben, im unteren Teil des Berichts ist dies besser ersichtlich. Als Hauptaufgabe haben wir uns vor allem der Aufgabe, einen Drive ohne jegliche Laserdaten etc. zu machen. Mit dieser Variante konnten wir ausschlieslich das Drive analysieren. Zum Schluss soll die Gripperchallenge umgesetzt werden da der Gripper unsere Hauptstärke ist.

### Ziele
==Wir haben uns drei Ziele festgelegt welche wie folgt lauten:==

***OnlyDrive A2B:*** Das OnlyDrive des Robis soll funktionieren. Er soll eine Koordinate, welche mittels MQTT übergeben wird, anfahren. Ohne Ausweichen oder sonstiges. Prinzipell wäre das die einfachste Aufgabe für den Robi was jedoch noch nicht gewährleistet ist.

***OnlyDrive with a-Star:*** Anders als beim A2B soll er nun auch ausweichen können, insofern ein Gegengstand im Weg steht. Zurzeit beziehen wir den A-Star, im vorherigen Jahr (2020) wurde ein D-Star erstellt welcher jedoch nicht funktioniert. Dieser kann deswegen ignoriert werden.

***OnlyDrive without Laser:*** Das Drive soll mit einem Übergabewert ohne Laser gestartet werden können. Mit dieser Methode können wir den Drive losgelöst vom Laser testen. Wir sind der Meinung dass, das Drive grundsätzlich gut funktioniert aber die Laserdaten oft Fehler verursachen. Wir wollen spätestens nach der Umsetzung das Drive analysieren damit wir wissen wie gut der Drive mit oder ohne Laser fährt.

Schlussendlich soll beim Starten des OnlyDrives gesagt werden ob der Robi mit oder ohne A-Star losfahren soll. So könnte man die einfachen funktionen testen bevor man das ganze Spiel startet. Ebenfalls soll es komplett losgelöst sein von der Refbox beziehungsweise vom Master.

> Nach gewisser Zeit haben wir die Gripper-Challenge auch noch als Auftrag angenommen

***Gripper-Challenge:*** Eigene Klasse aufbauen welche nur für die Gripper-Challenge gedacht ist. So kann man das Spiel mit einem seperaten Argument beginnen. Für diese Umsetzung wird eine StateMachine aufgebaut welche direkt dem Robi über MQTT die Koordinaten sowie die Approach-Befehle sendet damit dieser in den ApproachControl geht.

## Erkentnisse
Das OnlyDrive ist bisher per MQTT Topic "Target" gesteuert worden. Der Robi fährt zwar, jedoch macht er nicht das was er sollte. Wir haben die verschiedenen Topics studiert und uns schlussendlich auf das "FINAL" Topic konzentriert. Theoretisch ist dieses Topic genau das was wir brauchten. Der Robi soll nur von A nach B fahren was er auch macht. **Allerdings kann der Robi nicht retour fahren.** Übergibt man ihm Koordinaten welche in der Y-Achse weniger sind als seine aktuelle Position, so fährt er trotzdem weiter nach vorne. Nach einigen Analysen sind wir überzogen das es ein Fehler bei der Vektorrechnung sein muss.
Beim TARGET Topic sollte der Robi auch eine Position anfahren welche im übergeben wird, allerdings wird dort ständig die Koordinate neu gerechnet was dazu führt, dass er ganz komische Positionen anfährt und sich ständig dreht.

### Implementation des Lasers
Nachdem der Laser korrekt funktioniert hat ist der Robi sehr viel besser die Positionen angefahren. Das Drive wurde dank dem Laser deutlich verbessert. Wir gehen stark davon aus das der vorherige Laser nicht sehr effizient war und dem entsprechend das Drive auch weniger gut funktioniert hat.

### DriveOnly Case without Laser
Der nächste Ansatz ist ein weiteres Case im StartUp für das DriveOnly welches komplett lösgelöst vom Laser fahren soll. Dafür erstellen wir ein neues Case. Das Drive wird so nicht von aussen beinflusst und kann seperat getestet werden. Mit dieser Methode können wir ohne jeglichen Einfluss das Fahren testen.

## Problembehandlungen
### A2B - Rückwertsfahren (wurde verworfen da es die Apprachcontrol beinflusste)
Beim OnlyDrive fährt der Robi nicht so wie er sollte. Eine einfache Anfahrt von einem Startpunkt zum Zielpunkt ist mittlerweile gewährleistet, jedoch nur in der Y-Achse. Bisher war dies nicht immer der Fall.
Der Robi fährt jedoch in der Y-Achse nicht zurück. Vermutlich ist dies im FINAL Topic garnicht möglich oder er nimmt immer die Resetposition an sobald er das Target erreicht. Ziel wäre es das beim erreichen des Targets die aktuelle Position als Resetposition zu übernehmen, damit weitere Targets angesteurt werden können.

#### Lösungsansatz A2B

Es wurde festgestellt das die Methode "DirectVectorDrive()" keine Regelung in der x und der y Achse hatte. Deshalb wurden die Methoden "calcSpeedY()" und "calcSpeedX()" welche auf den miniPID zugreifen verwenden. So wurde verhindert das der Robotino überschiesst.
Auch die Toleranz musste um den Faktor 10 erhöht werden. Mit diesen Änderungen fährt der Robotino sein erstes Target an.

public boolean directVectorDrive() throws MqttException
    {
        actVector.calcVector(posCtrl.getActPos(), target);
        boolean targetReached = false;
        double distance = Math.sqrt(actVector.getX() * actVector.getX() + actVector.getY() * actVector.getY());

        double positioningTolerance;
        System.out.println(target+" target Coordinate");
        System.out.println(posCtrl.getActPos());

        if (pendingQuest == Quest.FINAL)
        {
            calcSpeedX(distance);         //additional PID controlling
            calcSpeedY(distance);         //additional PID controlling

           // positioningTolerance = 0.01F;
            positioningTolerance =0.1F;
        } else
        {
            positioningTolerance = 0.05F;
        }

--------
### Lösungsansatz Drive without laser
Wir haben überall, wo der Laser oder die DriveMap ins PIDDrive reinfunkt, diese erstmals auskommentiert. So konnten wir testen ob das Drive ohne die weiteren Klassen funktioniert.
Nach dem wir dies gewährleistet haben, haben wir den PIDDrive erweitert.
Damit nicht eine weitere Klasse des PIDDrives erstellt werden muss, haben wir dem Konstruktor einen Übergabewert mitgegeben. Wird dieser auf False gesetzt, funktioniert das PIDDrive wie gewohnt mit den nötigen Klassen. Wird der Konstruktor jedoch auf true gesetzt, so werden Laser oder DriveMap Klassen nicht aufgerufen. Anhand von IF-Bedingungen haben wir das gewährleistet.
Unten ist ein Code Beispiel zu sehen:

public PIDDrive(boolean driveOnlyWithoutLaser) throws FileNotFoundException, IOException
{       
    this.driveOnlyWithoutLaser=driveOnlyWithoutLaser;
    robotinoComAdapter = new RobotinoComAdapter(this);

if(driveOnlyWithoutLaser==false)
    {
        if (obstacleDetection.obstacleCloseBy(posCtrl.getActPos()) && pendingQuest == Quest.GOTOTARGET) {
            pidXaxis.setOutputLimits(approachSpeed / 3);
            pidYaxis.setOutputLimits(approachSpeed / 3);
            System.out.println("Object Close Reducing Speed");
        }
    }

try
        {
            if(driveOnlyWithoutLaser==false)      //obstacleDectection methoden werden nicht verwendet bei driveOnlyWithoutLaser
            {
                obstacleDetection.setBlockdTemp(posCtrl.getActPos());

                if ((drvMapCnt++ % 5) == 0) {
                    DriveMap.run();
                }
            }

            Coord actSpeedX = new Coord(posCtrl.getActSpeed().getVx() * 1000, 0, posCtrl.getActPos().getPhi());     //Koordinaten für Fahrwinkelberechnung
            Coord actSpeedY = new Coord(0, -posCtrl.getActSpeed().getVy() * 1000, posCtrl.getActPos().getPhi());    //Y minus weil der Laser im gegenuhrzeigersinn inkrementiert

            double movingDirection = actSpeedX.calcAngle(actSpeedY);
        
        

Der Robi fährt ohne Einfluss des Lasers nicht schlecht. Selbstverständlich sind Kollisionserkennung und solche Methoden nicht vorhanden.

## Änderungen

| Methoden | Zeile | Bemerkung |
| ----------------------- | --------------- |:------------------------------------------------------------------------ |
| PIDDrive im Konstruktor | PIDDrive Z.221 | Übergabewert im Konstruktor --> Drive mit oder ohne Laser |
| DirectVectorDrive() | PIDDrive Z.592 | Methode wird für den DriveOnly -> A2B gebraucht (wurde wieder gelöschen) |
| PositionTolerance | PIDDrive Z.630 | Toleranzwert wurde um das 10-Fache vergrössert |
| ObstacleDetection | PIDDrive Z.1268 | IF Bedingung für Drive ohne Laser |
___

## Wichtige Notizen
### Methoden

| Methoden | Zeile| Bemerkung|
| -------- | -------- | -------- |
| DirectVectorDrive()|PIDDrive Z.592| Methode könnte für den DriveOnly -> A2B gebraucht werden|

### Enums

| Quest | Zeile|Bemerkung|
| -------- | -------- | -------- |
| FINAL|PIDDrive Z.630 / Methode: AlignX() & AlignY()|Verwendbar für DriveOnly-> A2B|

### MQTT topics

- Die Topic String variablen sind im tpc.java file alle deklariert. Die das Topic welches wird für Onlydrive verwenden wollen heist "Final" (Z.93).
- Wenn bei der "messageArrived()" methode ein Topic gesendet wird welches mit dem String "Final" übereinstimmt wird mit einem Switch Case die das Quest.Final gesetzt (Z. 1184). Die Koordinaten welche angefahren werden müssen sind in der message des Topics vorhanden.

### REST
- im File "/robotinocfg/drive.xml" kann man mit dem Key "restAP" **0** für Robotino V3 oder **1** für Robotino V4 einstellen

### Config
#### Startpos
- im File kann "/robotino/gamePlay.xml" kann man die Startpositionen, welche beim initialiseren gesetzt werden verändern
myResetCoord = new Coord(2.5, 0.5, 0);

| IF manualStartUpPos = false | X | Y | Omega |
| ---- | --- | --- | --- |
| IF Cyan |
| **Robi 1** | 2.5 | 0.5 | 0 |
| **Robi 2** | 1.5 | 0.5 | 90 |
| **Robi 3** | 0.5 | 0.5 | 90 |
| IF Mangenta |
| **Robi 1** | 11.5 | 0.5 | 0 |
| **Robi 2** | 12.5 | 0.5 | 270 |
| **Robi 3** | 13.5 | 0.5 | 270 |

| IF manualStartUpPos = true | X | Y | Omega |
| ---- | --- | --- | --- |
| IF Entry = null | 2.5 | 0.5 | 0 |
| Type in your desired Value | x,x | y,y | omega |

# Gripper Challenge

Um am der Gripper-challenge teilzunehmen muss Gripper PID-Drive, und Leser miteinander zusammenfunktionieren.

- Es wurde festgestellt das die Approchcontroll-statemachine gar nie gestartet wird da dieser Init vom laser kommt welche Koordinaten auf das das Topic TPC.LASER_DATA senden muss so state die approchcontroll gestartet wird
- Mit dem alten Laserer funktionierte die approachcontrole relativ gut. Leider kam es aber ziemlich regelmässig zu exeptions beim Laser, weil er die Linie nicht erkannte.
- Tim wird uns nun die Laser daten über da Topic "robo/drive/laserdata"(TPC.LASER_DATA) schicken.
- Wenn die Laserdaten per MQTT ankommen können wir eine Statemachine für die Gripperchallenge programmieren.

## neue State-machine für Gripperchallange

Für die GripperChallenge wurde die neue Klasse GripperChallenge.java geschrieben. Die drei Position von welchen die Robotinos starten können, sind in der Klasse vorbereitet. Sie müssen lediglich mit dem richtigen Argument gestartet werden. Die Kommunikation erfolgt über MQTT. Es ist ein Counter vorhanden der dafür sorgt das die Robis den Ablauf genau dreimal machen.

Der Robotino muss mit Argument 3 gestartet werden (Drive-All). Wichtig ist das bei den Robotinos welche für die GripperChallenge verwendet werden das GripperChallenge Flag im "gamePlay.xml" auf TRUE steht. Dieses Flag sorgt dafür, dass die MPS-Stationen in blockierten Zonen sind.



Wenn alle Robis gestartet sind, muss noch einmal extern das Projekt mit Argument 9 gestartet werden. Diese sorgt dafür das die Statemachine für die Robis initialisiert wird.

Wie aud dem Bild zu sehen, ist default Robi 1 auf Pos1, Robi 2 auf Pos2 und Robi 3 auf Pos 3.

### MQTT Topics Send

| Topics | Message | Beschrieb |
| -------------------------- |:------------------------------------------------ |:--------------------------------------------------------------------------- |
| TPC.RESETPOS | Koordinaten in x,y und phi z.B (10.5;3.5;180) | Zu beginn der StateMachine wird jeder Robi auf seine Startpostion geresetet |
| TPC.Target | Koordinaten in x,y und phi | Zielkoordinate welche der Robi anfahren soll |
| TPC.APPROACH_MASCHINE_PICK | "6;true" für grippen / "5;false" für releasen | Wird geschickt für die ApproachControl zu starten |

### MQTT Topics Receive

| Topics | Message | Beschrieb |
| ----------------- |:------------------------------------- |:---------------------------------------------------------------------------------- |
| TPC.PICKED_PLACED | String "ready" | Wenn diese Nachricht empfangen wird, ist bekannt das die Approachcontrol fertig ist |
| TPC.INPOS | String "ready" (wird nicht überprüft) | Wenn diese Nachricht empfangen wrid, ist bekannt dass das Target erreicht ist |

### Beschrieb GripperChallenge
Es ist möglich die GripperChallenge mit drei, zwei oder mit einem Robotino zu bestreiten.
Die Roboter müssen vor dem Maschinen-Ausgang gestartet werden.
**Task:**
Der Roboter muss die Base von dem Maschinen Ausgang zurück zu dem Maschinen-Eingang fahren. Nach dem Vorgang darf eine Person die Base zurück zu dem Maschinen-Ausgang stellen. Der ganze Vorgang muss dreimal wiederholt werden. Die Maschinen-Position ist fix vorgegeben (siehe Bild).

### Punktvergebung

-----

## Gripper CodeSys

### Codesys Programm 2019
Im generellen läuft das Programm vom Jahr 2019 sehr zuverlässig. Das Problem ist aber das dieses Programm überhaupt nicht auf unsere ApproachControl abgestimmt ist.
Das Greifen und Releasen mit diesem Programm funktionierten nur, wenn die linke Kante als erste Kante detektiert wird. Dies führt zu sehr vielen Fehlern da die ApprachControl nicht immer gleich zu der Station heran fährt und manchmal und es auch oft vorkommt das die zweite Kante als erste Kante detektiert wird.

---------

### Codesys Programm 2020
Beim Programm von 2020 wurde theoretisch jeder Fall des Anfahrens abgedeckt. Praktisch kam es aber auch sehr oft zu Fehlern wie auf die Kante ausfahren oder neben dem Band abladen.
Es wurde erkannt das der Sensor, welcher die Kanten und das Band detektieren soll nicht immer zuverlässig die Ecken erkennt. Dies führt dazu das er öfters die falschen Y-Achsen Koordinaten in die Variablen "vsdi_firstEdge" und "vsdi_secondEdge" schrieb.


--------

#### Problemmöglichkeiten
##### falsche Refernzierung in der Y-Achse
Es wurde vermutet die Y-Achse sich nicht mehr richtig referenziert und man diese neu Kalibrieren muss.
Diese Vermutung wurde durch Debuggen und Manuellen Positionierung der Y-Achse widerlegt.

---------------------------

##### Range der Kantenerkennung könnte falsch sein
die zweite Vermutung war das die Range der Kanten nicht optimal sind und es vorkommen könnte, dass das Band als Kante erkannt wird.
Nach mehreren gezielt Veränderungen der Range konnte leider auch hier das Problem nicht behoben werden

##### Zykluszeit könnte Probleme machen das nicht die Aktuellen werden des Sensors gelesen werden
Diese Vermutung konnte schnell widerlegt werden, da die falschen Kanten Position in der Y-Achse immer zu früh und nicht zu spät in die Variablen übergeben werden.

#### Problemlösungsansatz
Das Problem konnte leider nicht genau eruiert werden, weil die Zeit fehlte. Nun musste ein "Workaround" gefunden werden.

--------
Da bekannt ist wo die Fehlerhaften Daten übergeben werden, wird versucht diese Abzufangen. Dies wird mit der Anpassung von einer IF-Bedingung in der Klasse getMiddle2Edge gemacht.


Neu darf die Position Mitte Band erst berechnet werden, wenn die Bedingung (zweite Kante >= erste Kante + Band breite) erfüllt ist. Dies funktioniert, aber es ist sicher keine dauerhafte Lösung.

- <mark>Dieser "Workaround" stellt sich als erfolgreich heraus. Der Ablauf ist nun sehr Prozesssicher aber die Effizienz lässt zu wünschen übrig. Es muss unbedingt herausgefunden werden Wiso die Positionen der Kanten falsch erkannt werden
Beim Austesten wurde aber noch ein weiterer Bug erkannt.

##### zweiter Fehler

Wenn der Gripper nur die erste Kante erkennen kann, weil die zweite nicht in der Range von der Y-Achse ist, wird vsdi_firstEdge beim dritten Versuch auf null gesetzt und die Y-Achsen Distanz von der ersten Kante wird in die Variable vsdi_secondEdge geschrieben. Diese führte dazu das der Gripper neben dem Band Grippen will.