Robo2004
10.03.2004, 15:17
Ich hab ein kleines Programm geschrieben in dem der Roboter eine definierte Strecke vorwärts und dann die gleiche Strecke rückwärts wieder zurück fahren soll. Wenn er dabei auf ein Hindernis trifft stoppt er und fährt natürlich auch nur den bis dort hin zurück gelegte weg zurück.
Das mit dem zurück fahren funktioniert nur leider nicht so wie es soll.
Ich merke den zurück gelegten Weg in der Variable moved und bei #MOVE_BACK sollte er dann diesen Weg zurücklegen aber er fährt immer nur ein kleinen taken.
Weiss jemand woran das liegt?
' Sascha Kröger
' 09.03.2004
' Programm Strecke2.bas
'
' ---------------------------------------------------------------------
' FUNKTION DES PROGRAMMS
'----------------------------------------------------------------------
' In diesem einfachen Beispiel wird der Roboter gestartet und fährt eine Strecke
' entsprechen den Wert von MAX_LENGTH. Wenn der Roboter die Strecke zurück-
' gelegt hat wird kurz auf den vier LEDs ein Lauflicht dargestellt. Anschließend
' fährt der Roboter die zuvor zurückgelegte strecke rückwärts wieder zurück.
' Wenn der Roboter beim vorwärts fahren auf ein Hindernis trifft, dann stoppt er
' gibt zweimal ein akustisches Signal aus und läst dann die vier LEDS alle
' ein paar mal Blicken. Anschließend fährt der Roboter die zuvor
' zurückgelegte strecke rückwärts wieder zurück.
'------------------------------------------------------------------------
'--------------------------
'------ 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)
'-------------------------------------------------------------------------------
define MAX_LENGTH 20 ' Konstanten für den Weg den der Roboter zurücklegen soll
define ROBOT_SPEED 120 ' Geschwindigkeit die der Roboter fahren soll
define moved byte ' Varibel für den tasächlich zurück gelegte weg
define k byte ' Schleifen Zähler
'---------- 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_LO
'------------ ANTRIEB -------------------------
REV_L=on:REV_R=on:SYS PLM_SLOW
'--------------------------------------------
gosub CLR_DISTANCE:gosub LEDSOFF:gosub move_fwd
#MOVE_FORWARD
pause 5:SYS COMNAV_STATUS
gosub L_DISTANCE
if (ACSL_F or ACSR_F)=on then goto WAYNOTFREE
if LBYTE=(MAX_LENGTH/4) then gosub LED1ON
if LBYTE=(2*MAX_LENGTH/4) then gosub LED2ON
if LBYTE=(3*MAX_LENGTH/4) then gosub LED3ON
if LBYTE=MAX_LENGTH then goto FORWARDFINISH
goto MOVE_FORWARD
'--------------------------------------------
' Wenn der Roboter auf ein hindernis gestoßen ist und nicht eine Strecke entsprechend
' MAX_LENGTH zurücklegen kann
#WAYNOTFREE
moved = LBYTE ' Den Weg den der Roboter zurückgelegt hat merken
SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:pause 10 ' Roboter Stoppen lassen
beep 368,10,0:pause 20
beep 368,10,0:pause 20
for k = 0 to 5
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:pause 5
gosub LEDSOFF:pause 5
next
goto FORWARDFINISH
'---------------------------------------------
#FORWARDFINISH
moved = LBYTE ' Den Weg den der Roboter zurückgelegt hat merken
gosub LED4ON
SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:pause 10 ' Roboter Stoppen lassen
for k = 0 to 5
gosub LED1ON:pause 5:gosub LED1OFF
gosub LED2ON:pause 5:gosub LED2OFF
gosub LED3ON:pause 5:gosub LED3OFF
gosub LED4ON:pause 5:gosub LED4OFF
gosub LED3ON:pause 5:gosub LED3OFF
gosub LED2ON:pause 5:gosub LED2OFF
next
gosub CLR_DISTANCE:gosub LEDSOFF:gosub move_rev
goto MOVE_BACK
#MOVE_BACK
pause 5:SYS COMNAV_STATUS
gosub L_DISTANCE
if LBYTE=(moved/4) then gosub LED1ON
if LBYTE=(2*moved/4) then gosub LED2ON
if LBYTE=(3*moved/4) then gosub LED3ON
if LBYTE=moved then goto stop
goto MOVE_BACK
'---------------------------------------------
#stop
gosub LED4ON
SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:end
'---------------------------------------------
' Roboter vorwärds fahren lassen
#move_fwd
SYS FWDR:SYS FWDL:SPEED_L=ROBOT_SPEED:SPEED_R=ROBOT_SPEED:retur n
'---------------------------------------------
' Roboter rückwärds fahren lassen
#move_rev
SYS REVR:SYS REVL:SPEED_L=ROBOT_SPEED:SPEED_R=ROBOT_SPEED:retur n
'---------------------------------------------
'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"
Das mit dem zurück fahren funktioniert nur leider nicht so wie es soll.
Ich merke den zurück gelegten Weg in der Variable moved und bei #MOVE_BACK sollte er dann diesen Weg zurücklegen aber er fährt immer nur ein kleinen taken.
Weiss jemand woran das liegt?
' Sascha Kröger
' 09.03.2004
' Programm Strecke2.bas
'
' ---------------------------------------------------------------------
' FUNKTION DES PROGRAMMS
'----------------------------------------------------------------------
' In diesem einfachen Beispiel wird der Roboter gestartet und fährt eine Strecke
' entsprechen den Wert von MAX_LENGTH. Wenn der Roboter die Strecke zurück-
' gelegt hat wird kurz auf den vier LEDs ein Lauflicht dargestellt. Anschließend
' fährt der Roboter die zuvor zurückgelegte strecke rückwärts wieder zurück.
' Wenn der Roboter beim vorwärts fahren auf ein Hindernis trifft, dann stoppt er
' gibt zweimal ein akustisches Signal aus und läst dann die vier LEDS alle
' ein paar mal Blicken. Anschließend fährt der Roboter die zuvor
' zurückgelegte strecke rückwärts wieder zurück.
'------------------------------------------------------------------------
'--------------------------
'------ 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)
'-------------------------------------------------------------------------------
define MAX_LENGTH 20 ' Konstanten für den Weg den der Roboter zurücklegen soll
define ROBOT_SPEED 120 ' Geschwindigkeit die der Roboter fahren soll
define moved byte ' Varibel für den tasächlich zurück gelegte weg
define k byte ' Schleifen Zähler
'---------- 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_LO
'------------ ANTRIEB -------------------------
REV_L=on:REV_R=on:SYS PLM_SLOW
'--------------------------------------------
gosub CLR_DISTANCE:gosub LEDSOFF:gosub move_fwd
#MOVE_FORWARD
pause 5:SYS COMNAV_STATUS
gosub L_DISTANCE
if (ACSL_F or ACSR_F)=on then goto WAYNOTFREE
if LBYTE=(MAX_LENGTH/4) then gosub LED1ON
if LBYTE=(2*MAX_LENGTH/4) then gosub LED2ON
if LBYTE=(3*MAX_LENGTH/4) then gosub LED3ON
if LBYTE=MAX_LENGTH then goto FORWARDFINISH
goto MOVE_FORWARD
'--------------------------------------------
' Wenn der Roboter auf ein hindernis gestoßen ist und nicht eine Strecke entsprechend
' MAX_LENGTH zurücklegen kann
#WAYNOTFREE
moved = LBYTE ' Den Weg den der Roboter zurückgelegt hat merken
SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:pause 10 ' Roboter Stoppen lassen
beep 368,10,0:pause 20
beep 368,10,0:pause 20
for k = 0 to 5
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:pause 5
gosub LEDSOFF:pause 5
next
goto FORWARDFINISH
'---------------------------------------------
#FORWARDFINISH
moved = LBYTE ' Den Weg den der Roboter zurückgelegt hat merken
gosub LED4ON
SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:pause 10 ' Roboter Stoppen lassen
for k = 0 to 5
gosub LED1ON:pause 5:gosub LED1OFF
gosub LED2ON:pause 5:gosub LED2OFF
gosub LED3ON:pause 5:gosub LED3OFF
gosub LED4ON:pause 5:gosub LED4OFF
gosub LED3ON:pause 5:gosub LED3OFF
gosub LED2ON:pause 5:gosub LED2OFF
next
gosub CLR_DISTANCE:gosub LEDSOFF:gosub move_rev
goto MOVE_BACK
#MOVE_BACK
pause 5:SYS COMNAV_STATUS
gosub L_DISTANCE
if LBYTE=(moved/4) then gosub LED1ON
if LBYTE=(2*moved/4) then gosub LED2ON
if LBYTE=(3*moved/4) then gosub LED3ON
if LBYTE=moved then goto stop
goto MOVE_BACK
'---------------------------------------------
#stop
gosub LED4ON
SYS FWDR:SYS FWDL:SPEED_L=0:SPEED_R=0:end
'---------------------------------------------
' Roboter vorwärds fahren lassen
#move_fwd
SYS FWDR:SYS FWDL:SPEED_L=ROBOT_SPEED:SPEED_R=ROBOT_SPEED:retur n
'---------------------------------------------
' Roboter rückwärds fahren lassen
#move_rev
SYS REVR:SYS REVL:SPEED_L=ROBOT_SPEED:SPEED_R=ROBOT_SPEED:retur n
'---------------------------------------------
'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"