IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII IIIIIIIIIIIIIIIIIIII
'IIIIIIIIII MOBILE ROBOT EVALUATION PROGRAM IIIIIIIIIIII
'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII IIIIIIIIIIIIIIIIIIIII
' EINFACHES PROGRAMM ZUR ÜBERWACHUNG DER AKKUSPANNUNG
' Benutzen Sie dieses Programm, wenn sie einen Überblick über den Zustand der Akkus
' haben wollen.Benutzen Sie diese Routine auch in eigenen Programmen um
' den Betrieb des Roboters sicher zu stellen bzw. rechtzeitig abzubrechen.
' Wenn die Akkuspannung unter 5,8 V absinkt, ist damit zu rechnen, dass
' der Controller einen Reset durchführt (bzw. einen permanenten Reset erfährt)
' -->Achtung:
' -->Plötzliche Wechsel der Laufrichtungen der Kette haben sehr hohe Stromspitzen
' -->(mehrere Ampere) zur Folge, was u.U. dazu führt, dass die Akkuspannung soweit
' -->einbricht, dass ein RESET erfolgt
' ---------------------------------------------------------------------
' FUNKTION DES PROGRAMMS
'----------------------------------------------------------------------
' Der Bezeichner SYS_VOLTS ist der A/D-Wandler, der die Akkuspannung misst.
' Ein Digit entspricht dabei 39,2 mV, der Messbereich geht bis 10 V
' Die Akkuspannung wird als Balkenanzeige mit den Leuchtdioden angezeigt.
' Die Schwellen sind folgendermassen eingestellt:
' LED 1 - blinkend:6,0 V (dauernd: 6,2 V)
' LED 2 - blinkend:6,4 V (dauernd: 6,6 V)
' LED 3 - blinkend:6,8 V (dauernd: 7,0 V)
' LED 4 - blinkend:7,2 V (dauernd: 7,4 V)
' Wenn keine LED mehr leuchtet ist der Akku erschöpft und sollte geladen werden.
' Damit die Ergebnisse reproduzierbar sind, sollte die Messung bei laufenden
' Motoren durchgeführt werden.
' Alternativ kann auch im Stand gemessen werden, wenn man z.B. den Wert 20
' in SPEED einträgt (wie in diesem Beispiel). Damit bewegt sich der Roboter noch nicht, aber es fliesst
' trotzdem ein nennenswerter Strom.
'------------------------------------------------------------------------
'RESOURCEN:
'Sie finden in allen Beispielen sämtliche Systemroutinen die zum Betrieb des
'Roboters notwendig sind.
'Viele davon greifen auf Hardwareresourcen zu, die Sie von C-Control her nicht
'kennen, deren genaue Funktionsweise aber im File "Project5_intern" erklärt ist.
'
' Für dieses Beispiel sind folgende SYSTEMROUTINEN relevant:
' POWER:
' gosub SUBSYS_PWR_ON ;Schaltet die Subsysteme EIN/AUS
' gosub SUBSYS_PWR_OFF
' AUSGABE:
' gosub LED1ON ;zum Ein/Ausschalten der LED1 analog auch LED 2bis 4
' gosub LED1OFF ;
' gosub LEDSOFF ;schaltet alle LEDS aus
'--------------------------
'------ 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 LEDPORT byte[6]
define L1_F bit[41]
define L2_F bit[42]
define L3_F bit[43]
define L4_F bit[44]
define L5_F bit[45]
define L6_F bit[46]
define L7_F bit[47]
define L8_F bit[48]
'--------------------------
'---- USER MEMORY -------
'--------------------------
define SVOLTS byte[6]
define PGM byte[7]
'--- 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---------------
SYS PLM_SLOW:gosub SUBSYS_PWR_ON:REV_L=off:REV_R=off
beep 368,10,0
ause 50
PGM=1
'--------------------------------------------
'--- SPANNUNGSÜBERWACHUNG FÜR DEN AKKU ---
'--------------------------------------------
gosub LEDSOFF
SPEED_L=20:SPEED_R=20
#WATCH_AKKU
if SYS_VOLTS > 189 then goto L8ON
if SYS_VOLTS > 184 then goto L8BLINK
if SYS_VOLTS > 179 then goto L7ON
if SYS_VOLTS > 173 then goto L7BLINK
if SYS_VOLTS > 168 then goto L6ON
if SYS_VOLTS > 163 then goto L6BLINK
if SYS_VOLTS > 158 then goto L5ON
if SYS_VOLTS > 153 then goto L5BLINK
if SYS_VOLTS > 146 then goto L4ON
if SYS_VOLTS > 141 then goto L4BLINK
if SYS_VOLTS > 136 then goto L3ON
if SYS_VOLTS > 130 then goto L3BLINK
if SYS_VOLTS > 125 then goto L2ON
if SYS_VOLTS > 119 then goto L2BLINK
if SYS_VOLTS > 114 then goto L1ON
if SYS_VOLTS > 109 then goto L1BLINK
goto WATCH_AKKU
#L8ON
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:gosub LED5ON:gosub LED6ON:gosub LED7ON
gosub LED8ON:goto WATCH_AKKU
#L8BLINK
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:gosub LED5ON:gosub LED6ON:gosub LED7ON
gosub LED8ON
ause 20:gosub L8OFF
ause 20:goto WATCH_AKKU
#L7ON
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:gosub LED5ON:gosub LED6ON
gosub LED7ON:goto WATCH_AKKU
#L7BLINK
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:gosub LED5ON:gosub LED6ON
gosub LED7ON
ause 20:gosub L7OFF
ause 20:goto WATCH_AKKU
#L6ON
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:gosub LED5ON:gosub LED6ON
goto WATCH_AKKU
#L6BLINK
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON:gosub LED5ON
gosub LED6ON
ause 20:gosub L6OFF
ause 20:goto WATCH_AKKU
#L5ON
gosub LED1ON:gosub LED2ON:gosub LED3ON:gosub LED4ON
gosub L5ON:goto WATCH_AKKU
#L5BLINK
gosub L1ON:gosub L2ON:gosub L3ON:gosub L4ON
gosub L5ON
ause 20:gosub L5OFF
ause 20:goto WATCH_AKKU
#L4ON
gosub L1ON:gosub L2ON:gosub L3ON
gosub L4ON:goto WATCH_AKKU
#L4BLINK
gosub L1ON:gosub L2ON:gosub L3ON
gosub L4ON
ause 20:gosub L4OFF
ause 20:goto WATCH_AKKU
#L3ON
gosub L1ON:gosub L2ON
gosub L3ON:goto WATCH_AKKU
#L3BLINK
gosub L1ON:gosub L2ON
gosub L3ON
ause 20:gosub L3OFF
ause 20:goto WATCH_AKKU
#L2ON
gosub L1ON:gosub L2ON:goto WATCH_AKKU
#L2BLINK
gosub L1ON
gosub L2ON
ause 20:gosub L2OFF
ause 20:goto WATCH_AKKU
#L1ON
gosub L1ON:goto WATCH_AKKU
#L1BLINK
gosub L1ON
ause 20:gosub L1OFF
ause 20:goto WATCH_AKKU
'IIIIIIIIIII LED DRIVER IIIIIIIIIIIIIIIII
#LED1ON
L1_F=on:goto EXTPORT_WRITE
#L1OFF
L1_F=off :goto EXTPORT_WRITE
#LED2ON
L2_F=on:goto EXTPORT_WRITE
#L2OFF
L2_F=off:goto EXTPORT_WRITE
#LED3ON
L3_F=on:goto EXTPORT_WRITE
#L3OFF
L3_F=off:goto EXTPORT_WRITE
#LED4ON
L4_F=on:goto EXTPORT_WRITE
#L4OFF
L4_F=off:goto EXTPORT_WRITE
#LED5ON
L5_F=on:goto EXTPORT_WRITE
#L5OFF
L5_F=off :goto EXTPORT_WRITE
#LED6ON
L6_F=on:goto EXTPORT_WRITE
#L6OFF
L6_F=off:goto EXTPORT_WRITE
#LED7ON
L7_F=on:goto EXTPORT_WRITE
#L7OFF
L7_F=off:goto EXTPORT_WRITE
#LED8ON
L8_F=on:goto EXTPORT_WRITE
#L8OFF
L8_F=off:goto EXTPORT_WRITE
#LEDSOFF
EXTPORT=EXTPORT and &H0F:goto EXTPORT_WRITE
#EXTPORT_WRITE
SYS SYSTEM
ulse 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"
Lesezeichen