-         

Ergebnis 1 bis 6 von 6

Thema: Der Stromsensor

  1. #1
    Benutzer Stammmitglied
    Registriert seit
    22.02.2009
    Beiträge
    49

    Der Stromsensor

    Anzeige

    Hallo.

    Nachdem sich das Problem mit der seriellen Schnittstelle zum Glück erledigt hat, konnte ich jetzt ein wenig weiterarbeiten. Als Schutzfunktion soll der Stromsensor abgefragt werden, und wenn zuviel strom gezogen wird (z.b Motor blockiert ), dann soll der roboter aufhören, und sich melden. Tut er aber nicht.

    Ja, der abschaltstrom ist extra so niedrig, weil ich es mal ausprobieren wollte. Und wenn er dann trotzdem ohne blockade mal stehen bleiben sollte, ist es auch nicht so schlimm. Davon geht er ja nicht kaputt.
    Übrigens, der ad1 funktioniert, denn die EINFUEHRUNG_STROMSENSOR.bas funktioniert, und der angezeigte strom steigt auch, wenn mann den roboter bei fahrt festhält, und leicht runterdrückt.

    Da aber keiner Gedankenlesen kann, hier der verwendete Code:
    Code:
    'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
    'IIIIIIIIII       MOBILE ROBOT EVALUATION PROGRAM          IIIIIIIIIIII
    'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
    '       EINFACHES BEISPIEL ZUR BENUTZUNG VON ANTRIEB UND ACS
    ' Der Robot ist jetzt bereit für seinen ersten Ausflug.
    ' Ideal hierfür ist ein Raum mit möglichst wenig Gelegenheiten zum Anecken.
    ' Der Roboter erkennt z.B. Stuhlbeine o.ä. nur sehr schlecht.
    ' Wenn Sie einen Goldhamster haben, der frei herumläuft, denken Sie daran,
    ' dieser Roboter ist für den Hamster ähnlich gefährlich, wie eine echte Raupe
    ' (mit blindem Fahrer), für Sie.
    ' ---------------------------------------------------------------------
    '                       FUNKTION DES PROGRAMMS
    '----------------------------------------------------------------------
    ' ACS
    ' In diesem Beispiel wird das ACS mit der SYSTEMROUTINE SYS COMNAV_STATUS
    ' abgefragt. Das Ergebnis steht in den Variablen SYSTEM_STATUS.
    ' Relevant für das ACS ist Bit 0 und Bit 1
    ' Bit 0 = linker ACS Sensor
    ' Bit 1 = rechter ACS Sensor
    ' Das Bit ist 1 wenn der jeweilige Sensor angesprochen hat.
    ' Das Ansprechen des linken Sensors wird mit LED 4, Ansprechen des rechten
    ' Sensors mit LED1 angezeigt.
    ' Wenn eine Kollisionswarnung z.B. links gemeldet wird, fährt der Roboter
    ' eine Rechtskurve (indem die rechte Kette langsamer läuft)bis das ACS
    ' wieder freie Fahrt signalisiert.
    ' Fährt der Roboter frontal auf ein Hindernis zu, so rotiert er auf der Stelle,
    ' bis das ACS freie Fahrt meldet
    
    ' MOTOREN
    ' Die Motoren werden über die Pulsweitenausgänge SPEED_L und SPEED_R
    ' des Computers gesteuert.Ein grosser Wert bedeutet eine grosse Spannung am Motor
    ' Und damit auch eine grosse Geschwindigkeit.
    ' Zusätzlich kann natürlich die Richtung jeder Kette bestimmt werden.
    '----------------------------------------------------------------------------------
    ' ACHTUNG:
    ' Der erste Schritt in der Initialisierung ihres Programms sollte immer die Zeile
    
    '                        REV_L=on:REV_R=on:SYS PLM_SLOW
    
    ' enthalten. Diese Zeile initialisiert die Ports für die Richtungsumschaltung
    ' des Antriebs und die PLM Frequenz.
    
    ' DER BETRIEB DER MOTOREN OHNE DIESE INITIALISIERUNG FÜHRT UNWEIGERLICH ZUR ZERSTÖRUNG
    ' DER ANTRIEBSELEKTRONIK !!
    '-----------------------------------------------------------------------------------
    '  Hinweis:
    '  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. Im Detail ist das von den jeweiligen Umständen
    '  und vor allem von der Qualität der Akkus abhängig. Sicherer ist es zwischen einem
    '  Laufrichtungswechsel ca. 200 ms Pause einzufügen.
    
    
    ' Die zugehörigen SYSTEMROUTINEN sind:
    ' SYS REVR           ;Kette rückwärts laufen lassen R/L
    ' SYS RERL
    ' SYS FWDR           ;Kette vorwärts laufen lassen R/L
    ' SYS FWDL
    ' SYS ROTR           ;Auf der Stelle nach R/L drehen
    ' SYS ROTL
    '------------------------------------------------------------------------
    '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
    
    ' AUSGABEN:
    ' gosub LED1ON   ;zum Ein/Ausschalten der LED1 analog auch
    ' gosub LED1OFF  ;LED 2bis 4
    ' gosub LEDSOFF  ;schaltet alle LEDS aus
    '
    ' ACS:
    ' gosub NO_ACS_INT   ;Interruptbetrieb abgeschaltet
    ' SYS ACS_HI         ;ACS HI POWER (SEHBEREICH ca. 60cm)
    ' SYS ACS_LO         ;ACS LO POWER (SEHBEREICH ca. 30cm)
    ' SYS ACS_MAX        ;ACS MAX POWER(SEHBEREICH ca. 100cm)
    ' SYS COMNAV_STATUS  ;Abfrage des ACS
    '
    ' ANTRIEB
    ' SYS REVR           ;Kette rückwärts laufen lassen R/L
    ' SYS RERL
    
    ' SYS FWDR           ;Kette vorwärts laufen lassen R/L
    ' SYS FWDL
    
    ' SYS FWD	     ;beide Ketten vorwärts
    ' SYS REV            ;beide Ketten rückwärts
    
    ' SYS ROTR           ;Auf der Stelle nach R/L drehen
    ' SYS ROTL
    '----------------------------------------------------------------
    '--------------------------
    '------ 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 TIM           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---------------
    gosub SUBSYS_PWR_ON
    beep 368,10,0:pause 50
    beep 368,10,0:pause 50
    beep 368,10,0:pause 50
    '---- SYSTEM OPERATION MODE (NO INTERRUPT) ----
    gosub NO_ACS_INT:beep 368,25,0:SYS ACS_HI
    '------------ ANTRIEB -------------------------
    REV_L=on:REV_R=on:SYS PLM_SLOW
    
    '--------------------------------------------
    '---         DEMO ANTRIEB                 ---
    '--------------------------------------------
    #loop
    pause 5:SYS COMNAV_STATUS
    '--------- LEDs ansteuern --------------------
    if ACSL_F=on then gosub LED4ON else gosub LED4OFF
    if ACSR_F=on then gosub LED1ON else gosub LED1OFF
    '--------- AUSWEICHMANÖVER EINLEITEN ---------
    if (ACSL_F and ACSR_F)=on then goto rotate
    if ACSL_F=on then goto move_right
    if ACSR_F=on then goto move_left
    SYS FWDR:SYS FWDL:SPEED_L=255:SPEED_R=255:goto loop
    
    '-------Notstop bei Blockierung---------------
    if SYS_CURRENT > 60 then goto emergency_stop 'Schwelle: ~ 600mA
    '-------- AUSWEICHEN NACH RECHTS -------------
    #move_right
    SPEED_R=0:SPEED_L=255:goto loop
    '-------- AUSWEICHEN NACH LINKS -------------
    #move_left
    SPEED_L=0:SPEED_R=255:goto loop
    '----------    ROTIEREN  --------------------
    #rotate
    SPEED_L=150:SPEED_R=150:SYS ROTR:goto loop
    '--------------NOTSTOP-------------------------
    #emergency_stop
    SPEED_L=0:SPEED_R=0
    beep 368,10,0:pause 50
    beep 368,10,0:pause 50
    beep 368,10,0:pause 50:end
    
    
    '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"

  2. #2
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    ... die Zeile "if SYS_CURRENT > 60 ..." wird nie ausgeführt, weil sich das "goto loop" zwei Zeilen höher befindet.

    Gruß Dirk

  3. #3
    Benutzer Stammmitglied
    Registriert seit
    22.02.2009
    Beiträge
    49
    Och nöö. Da hätte ich aber auch selber draufkommen können.
    Also wirklich. Fehler korrigiert, Akkus laden noch.

  4. #4
    Benutzer Stammmitglied
    Registriert seit
    22.02.2009
    Beiträge
    49
    So, die abschaltung funktioniert zumindest. Allerdings schon beim wenden. Werde den Wert erhöhen, und ggf. noch etwas zum rausrechnen der spitzen mit einbauen.

  5. #5
    Benutzer Stammmitglied
    Registriert seit
    22.02.2009
    Beiträge
    49
    Ich denke, ich probiere es erst einmal mit einer konditionierten Anpassung des schwellen wertes, da ja leider nur ein Stromsensor für das gesamte System vorhanden ist.
    1 Motor an: 400mA
    2 Motor an: 800mA
    Außerdem fehlen mir die pausen zwischen dem Umschalten der Drehrichtungen. Ich denke, das ist ein weiterer Grund, weshalb der immer mal einfach so abschaltet. ( wenn er gerade während einer Stromspitze misst oder so )

  6. #6
    Benutzer Stammmitglied
    Registriert seit
    22.02.2009
    Beiträge
    49
    Irgendwie kriege ich es nicht gebacken, jetzt schaltet er immer sofort ab.
    Ich habe den ACS genommen, um festzustellen, welcher Motor dreht, denn dieser Sensor wird ja verwendet, um die Motorgeschwindigkeiten zu setzen.

    Man ignoriere bitte das geblinke. Damit weiß ich, dass es ein Überstrom-Ereignis gegeben hat.
    Code:
    'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
    'IIIIIIIIII       MOBILE ROBOT EVALUATION PROGRAM          IIIIIIIIIIII
    'IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII
    '       EINFACHES BEISPIEL ZUR BENUTZUNG VON ANTRIEB UND ACS
    ' Der Robot ist jetzt bereit für seinen ersten Ausflug.
    ' Ideal hierfür ist ein Raum mit möglichst wenig Gelegenheiten zum Anecken.
    ' Der Roboter erkennt z.B. Stuhlbeine o.ä. nur sehr schlecht.
    ' Wenn Sie einen Goldhamster haben, der frei herumläuft, denken Sie daran,
    ' dieser Roboter ist für den Hamster ähnlich gefährlich, wie eine echte Raupe
    ' (mit blindem Fahrer), für Sie.
    ' ---------------------------------------------------------------------
    '                       FUNKTION DES PROGRAMMS
    '----------------------------------------------------------------------
    ' ACS
    ' In diesem Beispiel wird das ACS mit der SYSTEMROUTINE SYS COMNAV_STATUS
    ' abgefragt. Das Ergebnis steht in den Variablen SYSTEM_STATUS.
    ' Relevant für das ACS ist Bit 0 und Bit 1
    ' Bit 0 = linker ACS Sensor
    ' Bit 1 = rechter ACS Sensor
    ' Das Bit ist 1 wenn der jeweilige Sensor angesprochen hat.
    ' Das Ansprechen des linken Sensors wird mit LED 4, Ansprechen des rechten
    ' Sensors mit LED1 angezeigt.
    ' Wenn eine Kollisionswarnung z.B. links gemeldet wird, fährt der Roboter
    ' eine Rechtskurve (indem die rechte Kette langsamer läuft)bis das ACS
    ' wieder freie Fahrt signalisiert.
    ' Fährt der Roboter frontal auf ein Hindernis zu, so rotiert er auf der Stelle,
    ' bis das ACS freie Fahrt meldet
    
    ' MOTOREN
    ' Die Motoren werden über die Pulsweitenausgänge SPEED_L und SPEED_R
    ' des Computers gesteuert.Ein grosser Wert bedeutet eine grosse Spannung am Motor
    ' Und damit auch eine grosse Geschwindigkeit.
    ' Zusätzlich kann natürlich die Richtung jeder Kette bestimmt werden.
    '----------------------------------------------------------------------------------
    ' ACHTUNG:
    ' Der erste Schritt in der Initialisierung ihres Programms sollte immer die Zeile
    
    '                        REV_L=on:REV_R=on:SYS PLM_SLOW
    
    ' enthalten. Diese Zeile initialisiert die Ports für die Richtungsumschaltung
    ' des Antriebs und die PLM Frequenz.
    
    ' DER BETRIEB DER MOTOREN OHNE DIESE INITIALISIERUNG FÜHRT UNWEIGERLICH ZUR ZERSTÖRUNG
    ' DER ANTRIEBSELEKTRONIK !!
    '-----------------------------------------------------------------------------------
    '  Hinweis:
    '  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. Im Detail ist das von den jeweiligen Umständen
    '  und vor allem von der Qualität der Akkus abhängig. Sicherer ist es zwischen einem
    '  Laufrichtungswechsel ca. 200 ms Pause einzufügen.
    
    
    ' Die zugehörigen SYSTEMROUTINEN sind:
    ' SYS REVR           ;Kette rückwärts laufen lassen R/L
    ' SYS RERL
    ' SYS FWDR           ;Kette vorwärts laufen lassen R/L
    ' SYS FWDL
    ' SYS ROTR           ;Auf der Stelle nach R/L drehen
    ' SYS ROTL
    '------------------------------------------------------------------------
    '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
    
    ' AUSGABEN:
    ' gosub LED1ON   ;zum Ein/Ausschalten der LED1 analog auch
    ' gosub LED1OFF  ;LED 2bis 4
    ' gosub LEDSOFF  ;schaltet alle LEDS aus
    '
    ' ACS:
    ' gosub NO_ACS_INT   ;Interruptbetrieb abgeschaltet
    ' SYS ACS_HI         ;ACS HI POWER (SEHBEREICH ca. 60cm)
    ' SYS ACS_LO         ;ACS LO POWER (SEHBEREICH ca. 30cm)
    ' SYS ACS_MAX        ;ACS MAX POWER(SEHBEREICH ca. 100cm)
    ' SYS COMNAV_STATUS  ;Abfrage des ACS
    '
    ' ANTRIEB
    ' SYS REVR           ;Kette rückwärts laufen lassen R/L
    ' SYS RERL
    
    ' SYS FWDR           ;Kette vorwärts laufen lassen R/L
    ' SYS FWDL
    
    ' SYS FWD	     ;beide Ketten vorwärts
    ' SYS REV            ;beide Ketten rückwärts
    
    ' SYS ROTR           ;Auf der Stelle nach R/L drehen
    ' SYS ROTL
    '----------------------------------------------------------------
    '--------------------------
    '------ 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 TIM           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---------------
    gosub SUBSYS_PWR_ON
    gosub LEDSOFF
    beep 368,10,0:pause 50
    beep 368,10,0:pause 50
    beep 368,10,0:pause 50
    '---- SYSTEM OPERATION MODE (NO INTERRUPT) ----
    gosub NO_ACS_INT:beep 368,25,0:SYS ACS_HI
    '------------ ANTRIEB -------------------------
    REV_L=on:REV_R=on:SYS PLM_SLOW
    
    '--------------------------------------------
    '---         DEMO ANTRIEB                 ---
    '--------------------------------------------
    #loop
    pause 5:SYS COMNAV_STATUS
    '--------- LEDs ansteuern --------------------
    if ACSL_F=on then gosub LED4ON else gosub LED4OFF
    if ACSR_F=on then gosub LED1ON else gosub LED1OFF
    '--------- AUSWEICHMANÖVER EINLEITEN ---------
    if (ACSL_F and ACSR_F)=on then goto rotate
    if ACSL_F=on then goto move_right
    if ACSR_F=on then goto move_left
    SYS FWDR:SYS FWDL:SPEED_L=255:SPEED_R=255
    
    '-------NOTSTOP BEI BLOCKIERUNG---------------
    if (ACSL_F and ACSR_F)=on then goto both_motors_on
    if ACSL_F=on then goto one_motor_on
    if ACSR_F=on then goto one_motor_on
    #both_motors_on
    if SYS_CURRENT > 100 then goto emergency_stop
    #one_motor_on
    if SYS_CURRENT > 50 then goto emergency_stop else goto loop
    '-------- AUSWEICHEN NACH RECHTS -------------
    #move_right
    SPEED_R=0:SPEED_L=255:goto loop
    '-------- AUSWEICHEN NACH LINKS -------------
    #move_left
    SPEED_L=0:SPEED_R=255:goto loop
    '----------    ROTIEREN  --------------------
    #rotate
    SPEED_L=150:SPEED_R=150:SYS ROTR:goto loop
    '--------------NOTSTOP-------------------------
    #emergency_stop
    SPEED_L=0:SPEED_R=0
    gosub LED1ON
    gosub LED4ON
    pause 50
    beep 368,5,0:pause 20
    gosub LED1OFF
    gosub LED4OFF
    gosub LED2ON
    gosub LED3ON 
    beep 368,5,0:pause 20
    gosub LED2OFF
    gosub LED3OFF
    beep 368,5,0:pause 20
    gosub LED1ON
    gosub LED4ON
    gosub LED2ON
    gosub LED3ON:end
    '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"

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •