////////////////////////////////////////////////////////
//  AIBO-RENNEN für R-CODE110/111 - 2009-04-01
////////////////////////////////////////////////////////

:MAIN
    SET:walk:0              // Noch nicht loslaufen
    SET:state:0             // 1=Stehen 2=Sitzen 3=Liegen/Station
    SET:Power:1             // Motoren einschalten

:MAIN_RESTART
    SET:Wait:0              // Anzahl gleichzeitig laufender Aktionen zurücksetzen
    SET:Clock:0             // Interne Uhr zurücksetzen
    SET:stop:0              // Bewegungen nicht stoppen signalisieren
    SET:style:0             // Kein aktueller Laufstil
    SET:stable:1            // Aibo steht stabil
    SET:actions:1           // Anzahl Aktionen die gleichzeitig laufen
    SET:station:0           // Nicht auf Station
    SET:restart:0           // Noch nicht umgefallen
    SET:shutdown:0          // Noch nicht abschalten
    SET:oldstyle:0          // Kein vorheriger Laufstil
    SET:COLOR:PINK          // Lieblingsfarbe rosa
    SET:Touch_head_time:0   // Kopfsensor zurücksetzen
    CALL:STARTUP_POSITION   // Aufwachposition einnehmen

    GO:IDLE                 // In Ruhemodus gehen

////////////////////////////////////////////////////////

:IDLE
    STOP:LIGHT                      // Leuchtende Augen aus
    CALL:IDLE_POSITION              // Ruheposition einnehmen
    MOVE:TAIL:ABS:0:0:1000          // Schwanz gerade stellen
    MOVE:HEAD:ABS:0:0:0:1000        // Kopf gerade stellen

:IDLE_LOOP
    CALL:ATTENTION                  // Sinnesorgane prüfen
    IF:restart:<>:0:RESTART         // Umgefallen? Dann wieder aufstehen
    IF:shutdown:<>:0:SHUTDOWN       // Schlafen gehen? Dann abschalten
    IF:Cdt_npixel:>:32:TRACK_BALL   // Lieblingsfarbe sichtbar? Dann beobachten
    GO:IDLE_LOOP                    // Prüfungen wiederholen

////////////////////////////////////////////////////////

:IDLE_POSITION
    CALL:STATION                    // Auf Station?
    CASE:1:RETURN                   // Ja, dann nichts machen
    SWITCH:state                    // In welcher Lage?
    CASE:3:RETURN                   // Liegen? Dann nichts machen
    POSE:AIBO:oSleeping             // In Schlafposition gehen
    WAIT:AIBO                       // Warten bis Schlafposition eingenommen
    POSE:AIBO:oStation              // In Stationsposition gehen
    WAIT:AIBO                       // Warten bis Stationsposition eingenommen
    SET:state:3                     // Position merken
    RETURN

////////////////////////////////////////////////////////

:SEARCH_BALL
    CALL:STATION                    // Auf Station?
    CASE:1:GO:IDLE                  // Falls ja, dann sofort in Ruhemodus gehen

    STOP:LIGHT                      // Leuchtende Augen aus
    CALL:SEARCH_BALL_POSITION       // Suchposition einnehmen
    MOVE:TAIL:ABS:0:0:1000          // Schwanz gerade stellen

    SET:repeat:4                    // 4 Runden
    RND:Context:1:2                 // Links oder rechts nach Zufall
    CSET:Head_pan:>:+15:1           // Zeigt Kopf mehr als 15 Grad nach links?  Dann doch links
    CSET:Head_pan:<:-15:2           // Zeigt Kopf mehr als 15 Grad nach rechts? Dann doch rechts
    SET:leftright:Context           // Richtung setzen (nach links oder rechts)

:SEARCH_BALL_HEAD
    SET:time:60                     // Zeit fürs Kopfbewegen setzen
    SWITCH:leftright                // Richtung prüfen
    CASE:1:SUB:time:Head_pan        // Nach links?  Dann Kopfwinkel von Zeit abziehen
    CASE:2:ADD:time:Head_pan        // Nach rechts? Dann Kopfwinkel zu  Zeit dazuzählen
    MUL:time:30                     // Zeit mal 30 nehmen

    SWITCH:leftright                        // Richtung prüfen
    CASE:1:MOVE:HEAD:ABS:-15:+60:0:time     // Nach links?  Dann Kopf nach links  bewegen
    CASE:2:MOVE:HEAD:ABS:-15:-60:0:time     // Nach rechts? Dann Kopf nach rechts bewegen

    // Aibo bewegt seinen Kopf in die entsprechende Richtung.
    // Das Programm läuft währenddessen weiter!

    // Solange Aibo seinen Kopf bewegt ist die Variable "Wait" um 1 erhöht.

:SEARCH_BALL_LOOP
    CALL:ATTENTION                          // Sinnesorgane prüfen
    IF:restart:<>:0:RESTART                 // Umgefallen? Dann wieder aufstehen
    IF:shutdown:<>:0:SHUTDOWN               // Schlafen gehen? Dann abschalten
    IF:Cdt_npixel:>:32:SEARCH_BALL_TRACK    // Lieblingsfarbe sichtbar? Dann beobachten
    IF:Wait:>=:actions:SEARCH_BALL_LOOP     // Falls sich der Kopf noch bewegt
                                            // oder eine andere Aktion läuft,
                                            // die Prüfungen wiederholen

    // Aibo bewegt seinen Kopf jetzt nicht mehr.

    SWITCH:leftright                        // Letzte Richtung prüfen
    CASE:1:SET:leftright:2                  // Links?  Dann jetzt rechts
    CASE:2:SET:leftright:1                  // Rechts? Dann jetzt links

    SUB:repeat:1                            // Rundenzahl reduzieren
    IF:repeat:>:0:SEARCH_BALL_HEAD          // Falls noch eine Runde übrig ist,
                                            // Kopf in die neue Richtung drehen

    STOP:HEAD              // Kopf anhalten
    WAIT:HEAD              // Warten bis der Kopf angehalten ist
    GO:IDLE                // In Ruhemodus wechseln

:SEARCH_BALL_TRACK
    STOP:HEAD              // Kopf anhalten
    WAIT:HEAD              // Warten bis der Kopf angehalten ist
    GO:TRACK_BALL          // Dem Ball nachsehen

////////////////////////////////////////////////////////

:SEARCH_BALL_POSITION
    RETURN

////////////////////////////////////////////////////////

:TRACK_BALL
    CALL:TRACK_BALL_POSITION            // In Ballbeobachtungsposition gehen
    IF:Cdt_npixel:<:32:SEARCH_BALL      // Falls Ball nicht mehr sichtbar, dann Ball suchen

    MOVE:HEAD:C-TRACKING                // Dem Ball nachsehen
    MOVE:TAIL:ABS:90:0:1000             // Schwänzchen nach oben bewegen
    PLAY:LIGHT:hppy1eye:0               // Leuchtende Augen

:TRACK_BALL_LOOP
    CALL:ATTENTION                      // Sinnesorgane prüfen
    CALL:TRACK_BALL_HANDLER             // Weitere Prüfungen (siehe unten)
    IF:restart:<>:0:RESTART             // Umgefallen? Dann wieder aufstehen
    IF:shutdown:<>:0:SHUTDOWN           // Schlafen gehen? Dann abschalten
    IF:Wait:>=:actions:TRACK_BALL_LOOP  // Falls Aibo noch dem Ball nachsieht
                                        // oder eine andere Aktion läuft,
                                        // Prüfungen wiederholen

    // Aibo sieht seinem Ball jetzt nicht mehr nach.

    GO:SEARCH_BALL  // Wieder den Ball suchen

////////////////////////////////////////////////////////

:TRACK_BALL_POSITION
    CALL:STATION                        // Auf Station?
    CASE:1:RETURN                       // Ja, dann nichts machen
    SWITCH:state                        // In welcher Lage?
    CASE:1:RETURN                       // Stehen? Dann nichts machen
    POSE:AIBO:oStanding                 // In Stehposition gehen
    WAIT:AIBO                           // Warten bis Stehposition eingenommen
    SET:state:1                         // Position merken
    RETURN

////////////////////////////////////////////////////////

:TRACK_BALL_HANDLER
    CALL:STATION                              // Auf Station?
    CASE:1:RETURN                             // Ja, dann nichts machen

    IF:Head_pan:>:+60:TURN_LEFT_TRACK         // Kopf mehr als 60 Grad nach links?  Dann nach links drehen
    IF:Head_pan:<:-60:TURN_RIGHT_TRACK        // Kopf mehr als 60 Grad nach rechts? Dann nach rechts drehen
    IF:walk:<>:0:THEN                         // Falls laufen erlaubt:
        IF:Distance:>:200:WALK_FORWARD_TRACK  // Abstand mehr als 200? Dann vorwärts laufen
        IF:Cdt_npixel:>:32:TRACK_BALL_FOUND   // Falls Ball in Sichtweite? Dann Ziel erreicht!
    ENDIF
    RETURN

////////////////////////////////////////////////////////

:TRACK_BALL_FOUND                       // Ziel erreicht:
    SET:walk:0                          // Loslaufen nicht mehr erlaubt
    RETURN

////////////////////////////////////////////////////////

:TURN_LEFT_TRACK
    SET:repeat:24                       // 24 Wiederholungen
    SET:style:LEFT_TURN                 // Laufstil: Nach links drehen
    CALL:STAND_STABLE                   // Stabil stehen

:TURN_LEFT_TRACK_LOOP
    CALL:ATTENTION                      // Sinnesorgane prüfen
    MOVE:LEGS:STEP:style:0:1            // Einen Schritt machen
    WAIT:LEGS                           // Warten bis Schritt beendet
    SUB:repeat:1                        // Wiederholungen reduzieren
    IF:stop:<>:0:TURN_STOP              // Bewegungen stoppen?
    IF:repeat:<=:0:TURN_STOP            // Alle Wiederholungen durch?
    IF:Head_pan:<=:0:TURN_STOP          // Zeigt Kopf nach rechts?
    IF:Wait:<:actions:TURN_STOP         // Zu wenige Bewegungen am Laufen?
    GO:TURN_LEFT_TRACK_LOOP             // Nächsten Schritt machen

////////////////////////////////////////////////////////

:TURN_RIGHT_TRACK
    SET:repeat:24                       // 24 Wiederholungen
    SET:style:RIGHT_TURN                // Laufstil: Nach rechts drehen
    CALL:STAND_STABLE                   // Stabil stehen

:TURN_RIGHT_TRACK_LOOP
    CALL:ATTENTION                      // Sinnesorgane prüfen
    MOVE:LEGS:STEP:style:0:1            // Einen Schritt machen
    WAIT:LEGS                           // Warten bis Schritt beendet
    SUB:repeat:1                        // Wiederholungen reduzieren
    IF:stop:<>:0:TURN_STOP              // Bewegungen stoppen?
    IF:repeat:<=:0:TURN_STOP            // Alle Wiederholungen durch?
    IF:Head_pan:>=:0:TURN_STOP          // Zeigt Kopf nach links?
    IF:Wait:<:actions:TURN_STOP         // Zu wenige Bewegungen am Laufen?
    GO:TURN_RIGHT_TRACK_LOOP            // Nächsten Schritt machen

////////////////////////////////////////////////////////

:TURN_STOP
    SET:stable:1                        // Aibo steht schon stabil
    RETURN

////////////////////////////////////////////////////////

:WALK_FORWARD_TRACK
    SET:repeat:4                        // 4 Wiederholungen
    CALL:WALK_FORWARD_TRACK_STYLE       // Laufstil festlegen
    CALL:STAND_STABLE                   // Stabil stehen

:WALK_FORWARD_TRACK_LOOP
    CALL:ATTENTION                      // Sinnesorgane prüfen
    SET:Context:FORWARD                 // Laufrichtung geradeaus
    CSET:Head_pan:>:+15:LEFTFORWARD     // Falls Kopf mehr als 15 Grad nach links zeigt: Laufrichtung nach links vorwärts
    CSET:Head_pan:<:-15:RIGHTFORWARD    // Falls Kopf mehr als 15 Grad nach rechts zeigt: Laufrichtung nach rechts vorwärts
    MOVE:LEGS:STEP:style:Context:steps  // Schritte machen
    WAIT:LEGS                           // Warten bis Schritte beendet
    SUB:repeat:1                        // Wiederholungen reduzieren
    IF:stop:<>:0:WALK_STOP              // Bewegungen stoppen?
    IF:repeat:<=:0:WALK_STOP            // Alle Wiederholungen durch?
    IF:Distance:<:200:WALK_STOP         // Abstand kleiner als 200?
    IF:Head_pan:>:+60:WALK_STOP         // Zeigt Kopf mehr als 60 Grad nach links?
    IF:Head_pan:<:-60:WALK_STOP         // Zeigt Kopf mehr als 60 Grad nach rechts?
//  IF:Head_tilt:>:+10:WALK_STOP        // Zeigt Kopf mehr als 10 Grad nach oben?
    IF:Head_tilt:<:-65:WALK_STOP        // Zeigt Kopf mehr als 65 Grad nach unten?
    IF:Wait:<:actions:WALK_STOP         // Zu wenige Bewegungen am Laufen?
    GO:WALK_FORWARD_TRACK_LOOP          // Nächste Schritte machen

////////////////////////////////////////////////////////

:WALK_FORWARD_TRACK_STYLE
    RND:Context:1:2                     // Laufstil nach Zufall auswählen
    CSET:Distance:<:300:2               // Abstand nach vorne kleiner als 300? Dann normal laufen
//  CSET:Distance:>:900:2               // Abstand nach vorne größer als 900? Dann normal laufen
    CSET:oldstyle:==:TROT:2             // Letztes Mal gerobbt? Dann diesmal normal laufen
    CASE:1:GO:WALK_FORWARD_TRACK_TROT   // Robben?

:WALK_FORWARD_TRACK_DEFAULT             // Normal laufen:
    SET:style:DEFAULT                   // Laufstil auf normal setzen
    SET:steps:3                         // Immer 3 Schritte auf einmal machen
    RETURN

:WALK_FORWARD_TRACK_TROT                // Robben:
    SET:style:TROT                      // Laufstil auf robben setzen
    SET:steps:9                         // Immer 3 Schritte auf einmal machen
    RETURN

////////////////////////////////////////////////////////

:WALK_STOP
    SET:stable:0                        // Aibo steht instabil
    RETURN

////////////////////////////////////////////////////////

:STAND_STABLE
    IF:oldstyle:<>:style:THEN           // Falls Laufstil gewechselt hat:
        IF:stable:==:0:THEN             // Falls Aibo instabil steht:
            PLAY:LEGS:WSToStand         // Stabil stehen
            WAIT:LEGS                   // Warten bis Aibo stabil steht
            SET:stable:1                // Merken, daß Aibo jetzt stabil steht
        ENDIF
        SET:oldstyle:style              // Laufstil merken
    ENDIF
    RETURN

////////////////////////////////////////////////////////

:ATTENTION       // Sinnesorgane prüfen:
    SET:stop:0   // Bewegungen nicht stoppen signalisieren
    CALL:SENSOR  // Sinnesorgane prüfen
    RETURN

////////////////////////////////////////////////////////

:SENSOR
    IF:Emergency:<>:0:SENSOR_Emergency               // Notfall?
    IF:Gsensor_status:&:1:SENSOR_Falldown            // Umgefallen?
    IF:Touch_head_time:>:100:SENSOR_Touch_head_time  // Am Kopf gestreichelt?
    RETURN

:SENSOR_Emergency                       // Notfall:
    SET:shutdown:1                      // Schlafen gehen signalisieren
    SET:stop:1                          // Bewegungen stoppen signalisieren
    RETURN

:SENSOR_Falldown                        // Umgefallen:
    SET:restart:1                       // Aufstehen signalisieren
    SET:stop:1                          // Bewegungen stoppen signalisieren
    RETURN

:SENSOR_Touch_head_time                 // Am Kopf gestreichelt:
    SET:Touch_head_time:0               // Kopfsensor zurücksetzen
    CALL:STATION_RESET                  // Prüfen ob auf Station?
    SET:walk:1                          // Laufen erlaubt
    RETURN

////////////////////////////////////////////////////////

:STATION
    SWITCH:station                      // Auf Station?
    CASE:1:RET:station                  // Ja, dann 1 zurückgeben

:STATION_RESET
    SET:st:Status                       // Ermitteln ob auf Station
    AND:st:4                            // Bit ermitteln
    SWITCH:st                           // Bit prüfen
    CASE:0:SET:station:2                // 2=Normal
    CASE:ELSE:SET:station:1             // 1=Station
    RET:station                         // 1 oder 2 zurückgeben

////////////////////////////////////////////////////////

:STARTUP_POSITION
    CALL:STATION_RESET                  // Prüfen ob auf Station?
    CASE:1:POSE:AIBO:oStation           // Falls auf Station: In Stationsposition gehen
    CASE:ELSE:POSE:AIBO:oSleeping       // Andernfalls: In Schlafposition gehen
    WAIT:AIBO                           // Warten bis Stationsposition oder Schlafposition eingenommen
    SET:state:3                         // Position merken
    RETURN

////////////////////////////////////////////////////////

:RESTART
    QUIT:AIBO                           // Bewegungen sofort stoppen
    MOVE:AIBO:ReactiveGU                // Wieder aufstehen
    WAIT:AIBO                           // Warten bis aufgestanden
    SET:state:1                         // Stehposition merken
    GO:MAIN_RESTART                     // Neu beginnen

////////////////////////////////////////////////////////

:SHUTDOWN
    CALL:STATION                        // Auf Station?
    CASE:1:HALT                         // Ja, dann sofort abschlaten
    POSE:AIBO:oSleeping                 // In Schlafposition gehen
    WAIT:AIBO                           // Warten bis Schlafposition eingenommen
    HALT                                // Abschalten

////////////////////////////////////////////////////////
//  Copyright © Jürgen Reule 2008-2009  MADE IN GERMANY