\ Robby-Programm v. 0,6 (Schulz/Berlin) \ --- Endschalter zum Starten-Mega32 \ 1in Endschalter Boxen Ende \ 2in Endschalter Rempel \ 3in Endschalter Programm ein \ 4in Endschalter Kopf oben \ 5in Endschalter Hand links zu \ 1adc Abstandsmessung für Armsteuerung (prov.) \ 2adc Abstandsmessung vorn \ 1mot Motor rechts \ 2mot Motor links \ 1sv Servo Ventil Arm links \ 2sv Servo Abstand vorn \ 3sv Servo Kopf drehen \ 4sv Servo Ventil Kopf hoch \ 5sv Servo Handgelenk links \ 6sv Servo Hand links \ p1 Augen \ p2 Boxen \ p4 Pneumatik-Pumpe \ Variable handpos -- Position der Hand \ Variable aus -- Programm beenden \ Variable progstart -- Programm starten \ Variable zahler -- Stufung Abstands-Servo \ Variable status -- Status-Abstands-Servo \ Variable abstand -- Entfernung zum Objekt VAR handpos VAR aus VAR progstart VAR zahler VAR status VAR abstand : KOPF \ Kopf zum Hindernis drehen 2adc 30 > if \ wenn näher als 30 status C@ -50 > \ Servo-Position > -50 if \ dann 60 3sv \ drehe Kopf nach rechts für else \ nicht dann -60 3sv \ drehe Kopf nach links then else then ; : ZURUECK \ Programm zum zurück fahren 0 mmgo \ Motor stoppen 5 0,1s \ 0,5s warten -127 mmgo \ zurück fahren 10 0,1s \ für 1s 0 mmgo \ dann Motor aus 5 0,1s \ 0,5s warten ; : BOX \ Programm 1x boxen 2adc 100 > if \ Entfernung > 100 (sehr nahe) begin \ beginne p2on \ boxen bis 1in \ Sensor geschlossen until \ dann 4 0,1s \ Ausschaltverzögerung p2off \ boxen aus else then \ Ende ; : ARMVOR \ Programm Arm nach vorn 1adc 70 > if \ IR-Sensor > 70 5in if \ Wenn Hand zu dann 0 mmgo \ anhalten 20 0,1s \ warte 2s 10 handpos C! \ Position Hand speichern handpos C@ 6sv \ Hand auf p4on \ pumpe an -60 1sv \ Ventil für Arm runter 20 0,1s \ 2s bis weiter p4off \ pumpe aus else 0 mmgo \ Motor stop 60 1sv \ Ventil Arm hoch p4on \ Pneu-Pumpe an 150 0,1s \ für 15s p4off \ Pneu-Pumpe aus begin \ warte handpos C@ 1 - handpos C! \ handposition + 1 1 0,1s handpos C@ 6sv \ position auf servo 5in \ bis Hand geschlossen until \ dann 10 0,1s \ warte 1s then else then ; : REMPEL \ Programm Hindernis vorn 2in if \ wenn Sensor 2 ZURUECK \ zurück fahren -127 spin \ drehen nach links 10 0,1s \ für 1s 0 mmgo \ dann Stop 5 0,1s \ 0,5s warten else then \ Ende, weiter mit Prog. ; : VORN \ Programm IR-Sensor vorn auswerten 2adc 30 > if \ wenn IR > 30 dann BOX \ Abfrage Boxen ? (>100) 0 mmgo \ Motor stoppen 2 0,1s \ 0,2s warten status C@ -50 > \ Servo-Position > -50 if \ dann begin \ beginne 127 spin \ drehe nach rechts für 5 0,1s \ 0,5s 2adc 15 < \ ist Abstand < 15 until \ beende 0 spin \ drehen rechts 2 0,1s \ für 0,2s else \ linksdrehen bei < -50 (siehe oben) begin -127 spin 5 0,1s 2adc 15 < until 0 spin 2 0,1s then else \ IR < 30 127 mmgo \ weiter mit Power then \ Ende ; : IRSENS \ Programm IR-Sensor über Servo abfragen status C@ -50 > if \ Servo-Status größer -50 (-49 - 20) begin \ ja dann status C@ 1 - zahler C! \ Status - 1 = Zahler zahler C@ 2sv \ zahler = Servo-Position ARMVOR \ Programm Arm heben ? KOPF \ Kopfbewegung VORN \ Programm Sensor vorn ? 0 3sv \ Kopf mitte REMPEL \ Programm Hinderniss ? 5 MS \ 10ms warten zahler C@ status C! \ Wert zahler an status status C@ -110 = \ wenn Status = -110 until \ dann weiter.......... else begin \ siehe oben mit + und 20er Endwert status C@ 1 + zahler C! zahler C@ 2sv ARMVOR KOPF VORN 0 3sv \ Kopf mitte REMPEL 5 MS zahler C@ status C! status C@ 10 = until then 3in if 1 aus C! \ ist Programmtaste dann aus=1 else then ; : ENDE \ Endprogramm progstart C@ 5 = if \ wenn erststart 0 mmgo \ Motoren aus p2off p4off \ Antriebe aus 70 3sv \ Kopf rechts 10 0,1s \ warte -60 3sv \ kopf links 10 0,1s \ warte 10 3sv \ Kopf mitte 10 0,1s \ warte p1off \ Augen aus -60 4sv \ Ventil Kopf runter 50 0,1s \ 5s warten 0 progstart C! \ variable auf neustart else then ; : HAUPTPROGRAMM1 \ Hauptprogramm begin IRSENS \ Programm IR-Sensor über Servo abfragen REMPEL \ Programm Hindernis vorn aus C@ 1 = \ wenn Aus = 1 until \ dann ende ENDE ; : START \ Start des Programms begin 3in until 60 4sv \ ventil auf p4on \ pumpe an begin \ bis 4in \ taster 4 (Kopf hoch) until \ gedrückt, dann weiter 0 4sv \ ventil auf 0 p1on \ Augen an 70 3sv \ Kopf rechts 10 0,1s \ warte -60 3sv \ kopf links 10 0,1s \ warte 10 3sv \ Kopf mitte 10 0,1s \ warte 60 1sv \ Arm hoch 200 0,1s \ warte 20 s -60 3sv \ kopf links 10 0,1s \ warte -127 5sv \ Gelenk drehen 10 0,1s \ warte 10 5sv \ Gelenk zurück drehen 10 0,1s \ warte -127 6sv \ Hand zu 10 0,1s \ warte 10 6sv \ Hand auf 10 0,1s \ warte -60 1sv \ Arm runter p4off \ pumpe aus 10 3sv \ Kopf mitte 30 0,1s \ warte 3s 0 4sv \ Kopfventil zu 5 progstart C! \ Variable auf 5 HAUPTPROGRAMM1 \ Hauptprogramm starten \ else \ wenn nicht erststart \ then ; : ZUVAR \ Variablenerstzuweisung 0 progstart C! \ auf 0 0 aus C! \ AUS auf 0 0 handpos C! \ Handposition zu 10 0,1s \ 1s warten beep \ 1x piepen START ;