'{$STAMP BS2}

'{$PBASIC 2.5}

 

'Versionsbeschreibung

'Vers1.1        BS2 erkennt PSC, alle servos in Mittelstellung (PW750),Roboter läuft gradeaus

'Vers1.2        Schrittweite und Beschleinigungsrampe in Rutine über Variablen definiert

'                Signal an PIN8 Loslaufen, Signal an PIN12 anhalten

'Vers1.3        Testversion ohne Neuerung

'Vers1.4        Testversion ohne Neuerung

'Vers1.5        Variable "richt" definiert die Laufrichtung.

'                richt=1 vorwärtz

'                richt=0 rückwärtz

'Vers1.6        Nun sind wendemnöver möglich.

'                Die Variablen richtre und richtli bestimmen für beide Seiten differenziert die Laufrichtung.

'                Die Syntax der Eingaben ist gleich wie Vers.1.5 (1=vorwärtz, 0=rückwärtz).

'Vers1.7        Einige Verbesserungen im Programmcode

'

'Vers1.8        Vor- und Rückwertzlauf über RC

'Vers1.9        Vor- und Rückwertzlauf über RC mit verschiedenen Geschwindigkeiten

'Vers2.0        Lenkbewegungen im Gang möglich - definition der Lenkbewegung über Beschleunigungsrampe

'Vers2.1        Lenkbewegung im Gang nun über Pulsweiten gesteuert

'Vers2.2        Robbi kann nun auf der Stelle wenden

'Vers2.3        Fehler in Geschwindigkeitsregelung behoben

'Vers2.4        Geschwindigkeitsanzeige per 7Segmet-LED        0 - Stop

'                                                        1 - langsam rückwerts

'                                                        2 - mittel rückwerte

'                                                        3 - schnell rückwerts

'                                                        4 - langsam vorwerts

'                                                        5 - mittel vorwerts

'                                                        6 - schnel vorwerts       

'                                                        auf linker Seite liegendes U - rechtsherum drehen

'                                                        auf rechter Seite liegendes U - linksherum drehen

'                                       

Sdat                CON        15                        'PSC an Pin 15

Baud                CON        396                        'konstant mit 2400 boud

ch                 VAR        byte                        'IO-Port

pwhub                VAR         word                        'Pulsweite für Beinhub

pwvor                VAR        word                        'Pulsweite für Vorschub (Abfragewert)

pwvorre                VAR        word                        'Pulsweite für Vorschub (Rechenwert rechts)

pwvorli                VAR        word                        'Pulsweite für Vorschub (rechenwert links)

rahub                 VAR        byte                        'Beschleunigungsrampe für Beinhub

'ravorre         VAR        byte                        'Beschleunigungsrampe für Vorschub (rechts)

'ravorli         VAR        byte                        'Beschleunigungsrampe für Vorschub (links)

ravor                VAR        byte                        'Beschleunigungsrampe für Vorschub

pwneut                Var        word                        'Pulsweite für Servos in Mittelstellung

pwgo                VAR        word                        'Pulsweite für Vorschub (Rechenvariable in Routine)

'switch                Var        byte                        'Temporervariable für Tasterwertspeicherung

richtre                Var        byte                        'Bestimmt die Laufrichtung der rechten Beine

richtli                Var        byte                        'Bestimmt die Laufrichtung der linken Beine

rcvor                Var        word                        'Speicherwer, welcher aus der RC ausgelesen wird (Vorschubrichtung)

rcseite                Var        word                        'Speicherwer, welcher aus der RC ausgelesen wird (Lenkrichtung)

port                VAR        byte                        'Rechenvariable für 7-Segmentanzeige

leds                VAR        byte

pwvor=200                                'Pulsweite für Vorschub

pwvorre=200                                'Startwert

pwvorli=200                                'Startwert

pwhub=1000                                'Pulsweite für Hub

pwneut=750                                'Pulsweite Servomittelstellung

rahub=1                                        'Beschleunigungsrampe Hub

'ravorre=5                                'Beschleunigungsrampe Vorschub (rechts)

'ravorli=5                                'Beschleunigungsrampe Vorschub (links)

ravor=8                                        'Beschleunigungsrampe Vorschub

ch=1                                        'auzusteuernderIO-Port des PSC

'switch=0

richtre=1                                '1=vorwertz 0=rückwertz - rechte Beine

richtli=1                                '1=vorwertz 0=rückwertz - linke Beine

rcvor=0

rcseite=0

'

'Servocontroller initialisieren********************************************************************************************

buff        var        Byte(3)                                                        'temporere Variable mit 3 Byte

FindPSC:

Debug "Servocontroller (PSC) suchen!", CR

Serout  Sdat, Baud+$8000, ["!SCVER?",CR]                                'Versionsnr. mit VER? abfragen

Serin        Sdat, Baud, 500, FindPSC,[STR buff\3]

Debug        "PSC erkannt - Version: ", buff(0), buff(1), buff(2), CR

'GOSUB neutral                                                                'Servos in Mittelstellung

Mainstart:                                                                'Start des Hauptprogramms

'RC auslesen **************************************************************************************************************

PULSIN 14,0,rcvor                                'An PIN14 Signal Vorschubrichtung abfragen und in rcvor schreiben

DEBUG dec ? rcvor                                'Wert aus rcvor in Debugfenster ausgeben

PULSIN 9,0,rcseite                                'An PIN9 Signal Lenkrichtung abfragen und in rcseite schreiben

DEBUG dec ? rcseite                                'Wert aus rcvor in Debugfenster ausgeben

IF rcvor < 10000 THEN GOTO Mainstart                'Bei RC-aus Keine Bewegung

IF rcvor > 10500 THEN GOTO Mainstart

'Variablen für Pulsweiten schreiben (Ganggeschwindigkeit)****

pwvor=1

If rcvor < 10500 THEN                                'fast front

        pwvor=200

ENDIF

If rcvor < 10450 THEN                                'middel front

        pwvor=80

ENDIF

If rcvor < 10400 THEN                                'slow front

        pwvor=50

ENDIF

If rcvor < 10200 THEN                                'slow back

        pwvor=50

ENDIF

If rcvor < 10150 THEN                                'middel back

        pwvor=80

ENDIF

If rcvor < 10100 THEN                                'fast back

        pwvor=200

ENDIF

'Geschwindigkeitsangaben in 7Segmentanzeige********* *************************************************************************

'

'schnell vorwerts

If rcvor < 10100 then

'Anzeige loeschen

For Port=0 to 6 step 1

  low Port

Next

'Zahl 6 schreiben

high 1

high 2

high 0

high 4

high 5

high 6

ENDIF

'mittel vorwerts

If rcvor > 10100 then

'Anzeige loeschen

For Port=0 to 6 step 1

  low Port

Next

'Zahl 5 schreiben

high 1

high 2

high 0

high 4

high 5

ENDIF

'langsam vorwerts

If rcvor > 10150 then

'Anzeige loeschen

For Port=0 to 6 step 1

  low Port

Next

'Zahl 4 schreiben

high 1

high 3

high 0

high 4

ENDIF

'langsam rückwerts

If rcvor > 10300 then

'Anzeige loeschen

For Port=0 to 6 step 1

  low Port

Next

'Zahl 1 schreiben

high 3

high 4

ENDIF

'mittel rückwerte

If rcvor > 10400 then

'Anzeige loeschen

For Port=0 to 6 step 1

  low Port

Next

'Zahl 2 schreiben

high 2

high 3

high 0

high 6

high 5

ENDIF

'schnell rückwerts

If rcvor > 10450 then

'Anzeige loeschen

For Port=0 to 6 step 1

  low Port

Next

'Zahl 3 schreiben

high 2

high 3

high 0

high 4

high 5

ENDIF

'Wenn kein Vorschub, 0 ausgeben

leds=0

IF rcvor > 10200 Then

leds=1

ENDIF

IF rcvor > 10300 Then

leds=0

ENDIF

IF leds = 1 Then

For Port=0 to 6 step 1

  low Port

Next

For Port=1 to 6 step 1

 HIGH Port

Next

ENDIF

'auf der Stelle drehen anzeigen

'linksherum

IF leds = 1 THEN

        IF rcseite < 10200 Then

                'Anzeige loeschen

                For Port=0 to 6 step 1

                  low Port

                Next

                high 2

                high 1

                high 3

        ENDIF

ENDIF

'rechtsherum

IF leds = 1 THEN

        IF rcseite > 10300 Then

                'Anzeige loeschen

                For Port=0 to 6 step 1

                  low Port

                Next

                high 4

                high 5

                high 6

        ENDIF

ENDIF

'*******************************************************************************************************************************

pwvorre = pwvor                                        'aus der RC ausgelesene werte in die Vorschubvariablen schreiben

pwvorli = pwvor

'Lenkrichtung****

IF rcseite < 10200 THEN                                'ravor-Variablen für Lenkrichtung modifizieren

        pwvorre = pwvor + 30

        pwvorli = pwvor - 49

ENDIF

IF rcseite > 10300 THEN

        pwvorre = pwvor - 49

        pwvorli = pwvor + 30

ENDIF

'Laufrichtung****

IF rcvor < 10200 THEN

        richtre=1

        richtli=1

        GOTO laufen

ENDIF

IF rcvor > 10300 THEN

        richtre=0

        richtli=0

        GOTO laufen

                ELSE

                        'Rutine für wenden auf der Stelle

                               

                        IF rcseite > 10300 THEN

                        pwvorre = 100

                        pwvorli = 100

                        richtre = 0

                        richtli = 1

                        GOTO LAUFEN

                        ENDIF

                        IF rcseite < 10200 THEN

                        pwvorre = 100

                        pwvorli = 100

                        richtre = 1

                        richtli = 0

                        GOTO LAUFEN

                                ELSE

                                GOSUB neutral

                                GOTO Mainstart

                                ENDIF

ENDIF

'Laufroutine **************************************************************************************************************

LAUFEN:

 

'Hubbewegung1    +++++++++++++

DEBUG "Vorwärs gehen",CR

'Schritt I

pwgo = pwneut - (pwhub/2)                                                                '        Bein       

FOR ch=1 to 5 Step 4                                                                        '        1+3               

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]                '               

NEXT

pwgo = pwneut + (pwhub/2)

ch=3                                                                                         '        2               

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

pwgo = pwneut - (pwhub/2)

FOR ch=7 to 11 Step 4                                                                        '        4,6       

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

NEXT

pwgo = pwneut + (pwhub/2)

ch=9                                                                                        '        5               

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

Pause 100

'Vorschubbewegung+++++++++++

'Schritt II

IF richtre = 1 THEN

        pwgo = pwneut - (pwvorre/2)                                                                '

        ELSE

        pwgo = pwneut + (pwvorre/2)

ENDIF

FOR ch=0 to 4 Step 4                                                                        '        1+3               

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]                '               

NEXT

IF richtre = 1 THEN

        pwgo = pwneut + (pwvorre/2)

        ELSE

        pwgo = pwneut - (pwvorre/2)

        ENDIF

ch=2                                                                                        '        2               

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

IF richtli = 1 THEN

        pwgo = pwneut - (pwvorli/2)       

        ELSE

        pwgo = pwneut + (pwvorli/2)

        ENDIF

FOR ch=6 to 10 Step 4                                                                        '        4+6       

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

NEXT

IF richtli = 1 THEN

        pwgo = pwneut + (pwvorli/2)

        ELSE

        pwgo = pwneut - (pwvorli/2)

        ENDIF

ch=8                                                                                        '        5               

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

PAUSE 100

'Schritt III

pwgo = pwneut + (pwhub/2)

FOR ch=1 to 5 Step 4                                                                        '        1+3               

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]                '               

NEXT

pwgo = pwneut - (pwhub/2)

ch=3                                                                                         '        2               

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

pwgo = pwneut + (pwhub/2)

FOR ch=7 to 11 Step 4                                                                        '        4,6       

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

NEXT

pwgo = pwneut - (pwhub/2)

ch=9                                                                                        '        5               

SEROUT Sdat, Baud+$8000, ["!SC", ch, rahub, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

Pause 100

'Vorschubbewegung+++++++++++

'Schritt IV

IF richtre = 0 THEN

        pwgo = pwneut - (pwvorre/2)                                                                '

        ELSE

        pwgo = pwneut + (pwvorre/2)

        ENDIF

FOR ch=0 to 4 Step 4                                                                        '        1+3               

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]                '               

NEXT

IF richtre = 0 THEN

        pwgo = pwneut + (pwvorre/2)

        ELSE

        pwgo = pwneut - (pwvorre/2)

        ENDIF

ch=2                                                                                        '        2               

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

IF richtli = 0 THEN pwgo = pwneut - (pwvorli/2)

IF richtli = 1 THEN pwgo = pwneut + (pwvorli/2)

FOR ch=6 to 10 Step 4                                                                        '        4+6       

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

NEXT

IF richtli = 0 THEN

        pwgo = pwneut + (pwvorli/2)

        ELSE

        pwgo = pwneut - (pwvorli/2)

        ENDIF

ch=8                                                                                        '        5               

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwgo.LOWBYTE, pwgo.HIGHBYTE, CR]

PAUSE 100

goto Mainstart

'Subrutinen***************************************************************************************

'alle Servos auf Nullstellung und Zahl 0 in LED schreiben

 

neutral:

DEBUG "alle Servos in Mittelstellung fahren",CR

ch=0

FOR ch=0 to 12 Step 1

SEROUT Sdat, Baud+$8000, ["!SC", ch, ravor, pwneut.LOWBYTE, pwneut.HIGHBYTE, CR]

NEXT

DEBUG "Neutralstellung erreicht",CR

PAUSE 2000

RETURN

stop