Programm von:
Jan-Peter Müller 2.12.2003
Programm AchtV1_0.bas

FUNKTION DES PROGRAMMS In diesem einfachen Beispiel wird der Roboter gestartet und fährt eine Acht. Er soll auf der Ausgangsposition wieder zurückkehren. Es wird dazu der Wegstreckenzähler benutzt rechts und links



Code:
'                               Jan-Peter Müller
'                                   2.12.2003
'                               Programm AchtV1_0.bas
'
'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
'IIIIIIIIII                      Robby RP 5                IIIIIIIIIIII
'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII

' ---------------------------------------------------------------------
'                       FUNKTION DES PROGRAMMS
'----------------------------------------------------------------------
' In diesem einfachen Beispiel wird der Roboter gestartet und fährt eine Acht
' Er soll auf der Ausgangsposition wieder zurückkehren
' Es wird dazu der Wegstreckenzähler benutzt rechts und links
'
'------------------------------------------------------------------
' WEGSTRECKENZÄHLER
' gosub CLR_DISTANCE
' gosub L_DISTANCE
' gosub R_DISTANCE
'--------------------------
'------ I/O PORTS ---------
'--------------------------
'- INTERFACE LCD/EXTPORT --
define sdio         port[1]
define sclio        port[3]
define strobe       port[4]
'-- INTERFACE COM/NAV -----
define DATALINE     port[1]
define CLOCKLINE    port[2]
'--------------------------
'------ SENSORS ---------
'--------------------------
define LIGHT_L      ad[7]
define LIGHT_R      ad[6]
define SYS_VOLTS    ad[3]
define CHRG_CURRENT ad[2]
define SYS_CURRENT  ad[1]
define MIC          ad[4]
define TOUCH        ad[5]
'---------------------------
'------  DRIVE -------------
'---------------------------
define SPEED_L        da[1]
define SPEED_R        da[2]
define REV_L        port[6]
define REV_R        port[5]
'--------------------------
'---- SYSTEM MEMORY -------
'--------------------------
'--- INTERFACE BUFFER ----
define LBYTE         byte[1]
define HBYTE         byte[2]
define SUBCMD        byte[3]
'---- OPERATION DATA ------
define EXTPORT       byte[4]
define LED1_F        bit[29]
define LED2_F        bit[30]
define LED3_F        bit[31]
define LED4_F        bit[32]
define SYSTEM_STATUS byte[5]
define ACSL_F        bit[33]
define ACSR_F        bit[34]
define IR_F          bit[35]
'--------------------------
'----  USER MEMORY  -------
'--------------------------
define GP            byte[6]
define MAXLIGHT      byte[7]
define GP1           byte[8]
'--- SYSTEMROUTINEN -----------
define PLM_SLOW      &H01C4
define SYSTEM        &H01C9
define COMNAV        &H0154
'- ERWEITERTE SYSTEM ROUTINEN -
define REVR          &H0101   'ANTRIEB RECHTS RÜCKWÄRTS
define REVL          &H0106   'ANTRIEB LINKS RÜCKWÄRTS
define FWDR          &H010B   'ANTRIEG RECHTS VORWÄRTS
define FWDL          &H0110   'ANTRIEB LINKS VORWÄRTS
define ROTR          &H0115   'RECHTS DREHEN
define ROTL          &H0119   'LINKS DREHEN
define REV           &H011D   'RÜCKWÄRTS
define FWD           &H0121   'VORWÄRTS
define COMNAV_STATUS &H0125   'UPDATED ALLE FLAGS IM STATUS-REGISTER
define ACS_LO        &H01E1   'ACS POWER LO
define ACS_HI        &H01E9   '
define ACS_MAX       &H01F1   '
define SEND_TLM      &H014A   'SENDET TELEMETRIE (CH=HBYTE,DATEN=LBYTE)
define SEND_SPEEDR   &H0134   'SENDET TLM KANAL 8,PLM RECHTS
define SEND_SPEEDL   &H013A   'SENDET TLM KANAL 7,PLM LINKS
define SEND_SYSSTAT  &H0144   'SENDET TLM KANAL 0,SYSTEM STATUS
                              '(FLAGS für ACS,FWD/REV, ACS_LO/HI/MAX)




'---------- INIT---------------
        gosub SUBSYS_PWR_ON
        beep 368,10,0:pause 50
'---- SYSTEM OPERATION MODE (NO INTERRUPT) ----
        gosub NO_ACS_INT:beep 368,10,0:SYS ACS_MAX
'------------ ANTRIEB -------------------------
        REV_L=on:REV_R=on:SYS PLM_SLOW
'--------------------------------------------
'---         WEGSTRECKENZÄHLER            ---
'--------------------------------------------
        gosub CLR_DISTANCE:gosub LEDSOFF:gosub move_fwdl
#Schleife1
        
        SYS COMNAV_STATUS
        gosub R_DISTANCE
        if LBYTE>33 then goto Schleife2         '33 variieren
        if LBYTE>25 then SPEED_R=155
goto Schleife1

#Schleife2
        gosub LED1ON:gosub CLR_DISTANCE:gosub move_fwdr
#Schleife3
        
        SYS COMNAV_STATUS
        gosub L_DISTANCE
        if LBYTE>35 then goto ende              '35 variieren Ausgleich , unterschiedliche Motore, bzw. Schlupf
        if LBYTE>25 then SPEED_L=155
goto Schleife3
#ende
        gosub stop
        end
'--------------------------------------------
#stop
        gosub LEDSOFF:SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:return
#move_fwdl
        SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=255:return
#move_fwdr
        SYS FWDR:SYS FWDL:SPEED_L=255:SPEED_R=0:return
'---------------------------------------------



'IIIIIIIIIII LED DRIVER IIIIIIIIIIIIIIIII
#LED1ON
LED1_F=on:goto EXTPORT_WRITE
#LED1OFF
LED1_F=off :goto EXTPORT_WRITE
#LED2ON
LED2_F=on:goto EXTPORT_WRITE
#LED2OFF
LED2_F=off:goto EXTPORT_WRITE
#LED3ON
LED3_F=on:goto EXTPORT_WRITE
#LED3OFF
LED3_F=off:goto EXTPORT_WRITE
#LED4ON
LED4_F=on:goto EXTPORT_WRITE
#LED4OFF
LED4_F=off:goto EXTPORT_WRITE
#LEDSOFF
EXTPORT=EXTPORT and &H0F:goto EXTPORT_WRITE
#EXTPORT_WRITE
SYS SYSTEM:pulse STROBE:RETURN

'IIIIII SYSTEMROUTINEN COMM/NAV SYSTEM  IIIII

#GET_IRDATA
SUBCMD=1:sys COMNAV:return
#SEND_IRDATA
SUBCMD=0:sys COMNAV:return
#RC5
SUBCMD=4:sys COMNAV:LBYTE= HBYTE and &HFC
HBYTE=00:SUBCMD=2:sys COMNAV:return
#RC5_INT
SUBCMD=4:sys COMNAV:LBYTE= (HBYTE and &HFE)or 2
HBYTE=00:SUBCMD=2:sys COMNAV:return
#REC80
SUBCMD=4:sys COMNAV:LBYTE= (HBYTE or &H01)and&HFD
HBYTE=00:SUBCMD=2:sys COMNAV:return
#REC80_INT
SUBCMD=4:sys COMNAV:LBYTE= (HBYTE or &H03)
HBYTE=00:SUBCMD=2:sys COMNAV:return

'IIIIII   SYSTEMROUTINEN SYSTEM  IIIIIIIIIIII
#NO_ACS_INT
SUBCMD=4:sys COMNAV:LBYTE= HBYTE and &HFB
HBYTE=00:SUBCMD=2:sys COMNAV:return
#ACS_INT_200
SUBCMD=4:sys COMNAV:LBYTE= HBYTE or &H04
HBYTE=50:SUBCMD=2:sys COMNAV:return
#SUBSYS_PWR_ON
sdio=on:sclio=on:strobe=off:EXTPORT=(EXTPORT and &HFE)or 8
sys SYSTEM:return
#SUBSYS_PWR_OFF
EXTPORT=(EXTPORT and &HF7)or 1:sys SYSTEM
deact sdio:deact sclio:return
#CLR_DISTANCE
SUBCMD=3:sys COMNAV:return
#L_DISTANCE
SUBCMD=6:sys COMNAV:return
#R_DISTANCE
SUBCMD=7:sys COMNAV:return
'---------COM/NAV GERÄTEREIBER -------------
'syscode "p5driv.s19"