This source code has been written in C-Control-I-Basic.

Please refer to C-Control-I Documentation.

Since the project is still on-going the day I wrote this HTML-Site,

the Source-Code may be out dated.

Please try to find the latest version in the Download Section.

'********************************************************************
'
' C-Control/BASIC yarp v01.bas
'
' Systemvoraussetzungen:
'
' - Application Board
' - YARP Aufbau (4 E-Motoren, 4 Transistoren-H-Bruecken, 3 Ultraschall-Sensoren
' - diese Software
' - 6 Volt Akku fuer E-Motoren
' - 7,2 Volt Akku für Steuerung + externe Verbraucher
' - TNC2 Modem
' - Funkgerät
'********************************************************************

define schrittmotor_port BYTEPORT [2]
schrittmotor_port = &H00 ' schrittmotor port wird null gesetzt. muss solange als erstes kommen, wie kein pull down an den ausgängen vorhanden ist.

define pts_buffer_left byte
define pts_buffer_right byte
define ser_in_byte byte
define zaehler byte
define akkuzaehler byte
define schritt_anzahl word
define setting_manuell_fahren bit[192]
define exit_bit bit [191]
define diagnose_mode_bit bit [190]
define akkudiagnose_ueberwachungs_bit bit [189]
define io_control bit [188]
define hupe bit [187]
define hindernis_umfahren bit [186]

define pts_sensor_left AD[2] ' linker ultraschallsensor
define pts_sensor_right AD[1] ' rechter ultraschallsensor
define e_motor_sum_current AD[3] 'summenstrom der e-motoren
define engine_akku_voltage AD[4] 'spannung des e-motoren-akkus, typ. 6 volt
define control_unit_voltage AD[5] ' spannung der steuerung, typ. 12 volt

'*** ASCII-Codes ***
define A_ &H41 : define B_ &H42 : define C_ &H43 : define D_ &H44 : define E_ &H45 : define F_ &H46 : define G_ &H47 : define H_ &H48 : define I_ &H49 : define J_ &H4A
define K_ &H4B : define L_ &H4C : define M_ &H4D : define N_ &H4E : define O_ &H4F : define P_ &H50 : define Q_ &H51 : define R_ &H52 : define S_ &H53 : define T_ &H54
define U_ &H55 : define V_ &H56 : define W_ &H57 : define X_ &H58 : define Y_ &H59 : define Z_ &H5A
define command_ &H63

define null_ &H30 : define eins_ &H31 : define zwei_ &H32 : define drei_ &H33 : define vier_ &H34 : define fuenf_ &H35 : define sechs_ &H36 : define sieben_ &H37
define acht_ &H38 : define neun_ &H39

define klammer_auf_ &H28 : define klammer_zu_ &H29 : define SLASH_ &H2F : define LEERZ &H20 : define MINUS &H2D : define NULL &H30
define GLEICH &H3D : define PUNKT &H2E : define AUSRUFEZEICHEN &H21
define ESC_ &H1B : define LF_ &H0A : define KLAMMERAFFE_ &H40
define STX_ &H02 : define ETX_ &H03 : define SYN_ &H16 : define XON_ &H11 : define XOFF_ &H13
define CAN_ &H19 : define CR_ &H0D

'-------------------------

define p1 port[1]
define p2 port[2]
define p3 port[3]
define p4 port[4]
define p5 port[5]
define p6 port[6]
define p7 port[7]
define p8 port[8]
define motoren_port BYTEPORT [1]

define p9 port [9]
define p10 port [10]
define p11 port [11]
define p12 port [12]
' define schritt0 &B11110000
'define schritt1 &B00000101
'define schritt2 &B00001001
'define schritt3 &B00001010
'define schritt4 &B00000110

'---defines für schwellwerte
define unterstrom_einzelmotor 5 ' ist noch zu checken !
define ueberstrom_einzelmotor 100 ' ist noch zu checken !
define hindernis_schwelle 25
define fehlerton 568 : define fehlertonlaenge 40
define unterspannung_control_unit 100
define unterspannung_e_motoren 55

#init
BAUD R9600
beep 568,10,0 : beep 250,10,0 ' alive ton
'HANDSHAKE ON ' kann ggfs spaeter entfallen
gosub TNC_INIT ' TNC wird für 1k2 parameter geladen
gosub connect_host ' der host des yarps wird versucht direkt zu connecten. wenn "via" benutzt werden soll, dann ist dies zu codieren.


#init_vars
setting_manuell_fahren= -1 'fahrzeug fährt nicht los, bis in settings manuell fahren auscodiert wurde
ser_in_byte = 250 'ser_in_byte wird mit 250 geladen, sodaß keine falschen interpretationen gemacht werden.
diagnose_mode_bit = 0 'keine diagnose-ausgaben
akkudiagnose_ueberwachungs_bit = 0 'defaultmaessig keine akku-überwachung, sollte später auf 1 gesetzt werden (final version)
io_control=0 'defaultmaessig ist io_control aus
hupe = 1 'default: hupe wird aktiviert bei hindernisserkennung
hindernis_umfahren = 1 'default: hindernis erkennen und abhngig von bit setting_manuell_fahren umfahren oder nicht

#initausgabe
print "yarp: ?:>";:put CR_

#main_loop
gosub hindernis
goto RX : #return_RX
gosub akku_diagnose
gosub TX

goto main_loop

end

#RX

if RXD then get ser_in_byte else goto return_RX
if ser_in_byte <> C_ then goto return_RX
if RXD then get ser_in_byte else goto return_RX
if ser_in_byte > 64 AND ser_in_byte < 91 then ser_in_byte = ser_in_byte - 65 else goto return_RX 'grossbuchstaben werden akzeptiert
on ser_in_byte gosub a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p,q,r,s,t,u,v,w,x,y,z
ser_in_byte = 250 'ser_in_byte wert auf ungültig setzen
goto return_RX

#a
return
#b
return
#c
return

#d
gosub diagnose
return

#e
return
#f
return
#g
return
#h
gosub hupe
return
#i 'I/O Controls
io_control = 1
gosub iocontrol
io_control = 0
return
#j
return
#k
return
#l
return
#m
return
#n
return
#o
return
#p
return
#q
return
#r
return
#s
gosub settings
return
#t
return
#u
return
#v
return
#w
return
#x
return
#y
return
#z
return


#TX
'put CR_
'print "yarp:"
'print "CS,...:?>"

return

#hupe
beep 600,20,10
beep 600,20,10
return

#iocontrol
if RXD then get ser_in_byte else return
if ser_in_byte < 65 OR ser_in_byte > 90 then return
if ser_in_byte = B_ AND setting_manuell_fahren= -1 then gosub rueckwaerts
if ser_in_byte = F_ AND setting_manuell_fahren= -1 then gosub vorwaerts_ohne_stop
if ser_in_byte = L_ AND setting_manuell_fahren= -1 then gosub links
if ser_in_byte = R_ AND setting_manuell_fahren= -1 then gosub rechts
if ser_in_byte = C_ then gosub io_control_schritt_motor
if ser_in_byte = S_ then gosub stop 'S wie stop
print "yarp: ?:>";:put CR_

return

#io_control_schritt_motor
print "<yarp>: Alle Maschinen Stop !";:put CR_
exit_bit = 0 ' das menu wird nicht verlassen, bevor das bit = 1 ist.
gosub stop
#loop_schritt_menu
print "Schrittmotor Menu:"
print "0. Menu verlassen (Exit)";:put CR_
print "1. Rechts Drehen";:put CR_
print "2. Links Drehen";:put CR_
print "?>"; : put CR_

#loop_schritt_abfrage 'warten bis ein gültiges byte empfangen wurde
if RXD then get ser_in_byte else goto loop_schritt_abfrage 'warten bis ein byte empfangen wurde, dann abfrage, ob gültig

if ser_in_byte > 47 and ser_in_byte < 58 then ser_in_byte = ser_in_byte - 48 else goto loop_schritt_abfrage

on ser_in_byte gosub s_exit,schritt_motor_rechtsdrehen,schritt_motor_linksdrehen

if exit_bit = 0 then goto loop_schritt_menu 'solange dieses bit = 0 wird im settingsmenu geblieben
ser_in_byte = 250

return

#schritt_motor_rechtsdrehen
if RXD then get ser_in_byte

print "Wieviel Schritte nach rechts (4..200)?";:put CR_

#schritt_motor_rechs_schritte_abfrage
print "?:>";:put CR_

input schritt_anzahl

if schritt_anzahl < 4 OR schritt_anzahl > 200 then goto schritt_motor_rechs_schritte_abfrage

for zaehler = 1 to ((schritt_anzahl / 4) + (schritt_anzahl mod 4))

p9=0: p10=1: p11=0: p12=1 ' schritt 1
pause 1
p9=1: p10=0: p11=0: p12=1 ' schritt 2
pause 1
p9=1: p10=0: p11=1: p12=0 ' schritt 3
pause 1
p9=0: p10=1: p11=1: p12=0 ' schritt 4
pause 1
next
p9=0: p10=0: p11=0: p12=0 ' schritt 0
return

#schritt_motor_linksdrehen
if RXD then get ser_in_byte
print "Wieviel Schritte nach links (4..200)?";:put CR_

#schritt_motor_links_schritte_abfrage
print "?:>";:put CR_

input schritt_anzahl

if schritt_anzahl < 4 OR schritt_anzahl > 200 then goto schritt_motor_links_schritte_abfrage

for zaehler = 1 to ((schritt_anzahl / 4) + (schritt_anzahl mod 4))
p9=0: p10=1: p11=1: p12=0 ' schritt 4
pause 1
p9=1: p10=0: p11=1: p12=0 ' schritt 3
pause 1
p9=1: p10=0: p11=0: p12=1 ' schritt 2
pause 1
p9=0: p10=1: p11=0: p12=1 ' schritt 1
pause 1
next
p9=0: p10=0: p11=0: p12=0 ' schritt 0
return

#diagnose

print "<yarp>: Alle Maschinen Stop !";:put CR_
diagnose_mode_bit = -1
gosub stop
ser_in_byte = 250
exit_bit = 0 ' das menu wird nicht verlassen, bevor das bit = 1 ist.

#loop_diagnose_menu
print "Diagnose Menu:"
print "0. Menu verlassen (Exit)","1. E-Motoren";:put CR_
print "2. Ultra-Schall-Sensoren","3. Akkus";:put CR_
print "4. Video","5. TNC","6. Fehlercodes";:put CR_
print "?>"; : put CR_

#loop_diagnose_abfrage 'warten bis ein gültiges byte empfangen wurde
if RXD then get ser_in_byte else goto loop_diagnose_abfrage 'warten bis ein byte empfangen wurde, dann abfrage, ob gültig

if ser_in_byte > 47 and ser_in_byte < 58 then ser_in_byte = ser_in_byte - 48 else goto loop_diagnose_abfrage
akkudiagnose_ueberwachungs_bit = -1 : akkuzaehler = 255 'falls akkudiagnose ausgewaehlt wird
on ser_in_byte gosub s_exit,e_motoren_diagnose,ultraschall_sensoren_diagnose,akku_diagnose

if exit_bit = 0 then goto loop_diagnose_menu 'solange dieses bit = 0 wird im settingsmenu geblieben
akkudiagnose_ueberwachungs_bit = 0 'falls akkudiagnose ausgewaehlt wird
return

#ultraschall_sensoren_diagnose
print "<yarp> Stelle Hindernis (30 cm breit) in 20 cm Entfernung senkrecht";:put CR_
print " vor das Fahrzeug (v/h).";:put CR_
print " Wenn bereit, dann weiter mit <W>";:put CR_
#ultraschall_hinweis_bestaetigung_falsche_eingabe
if ser_in_byte <> CR_ then print ":?>";:put CR_
#ultraschall_hinweis_bestaetigung
if RXD then get ser_in_byte else goto ultraschall_hinweis_bestaetigung
if ser_in_byte <> W_ then goto ultraschall_hinweis_bestaetigung_falsche_eingabe
if pts_sensor_right > hindernis_schwelle AND pts_sensor_left > hindernis_schwelle then print "<yarp> L:";pts_sensor_left;" R:";pts_sensor_right;" => Sensoren O.K !"; else print "L:";pts_sensor_left;" R:";pts_sensor_right;" => Sensoren defekt !";
put CR_
return

#e_motoren_diagnose
p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0
pause 10
print "<yarp>: Starte Prüfsequenz der E-Motoren-Diagnose ...";:put CR_
p1=1
p2=0
p3=0
p4=0
pause 3
print "Strom vorne links:";e_motor_sum_current,
' pause 10
if e_motor_sum_current < unterstrom_einzelmotor OR e_motor_sum_current > ueberstrom_einzelmotor then print "Stromkreis Motor vorne links ist defekt !"; else print "Stromkreis Motor vorne links ist O.K. !";
put CR_
p1=0
p2=1
p3=0
p4=0
pause 3
'steuere vorne_rechts an, alle anderen aus
print "Strom vorne rechts:";e_motor_sum_current,
' pause 10
if e_motor_sum_current < unterstrom_einzelmotor OR e_motor_sum_current > ueberstrom_einzelmotor then print "Stromkreis Motor vorne rechts ist defekt !"; else print "Stromkreis Motor vorne rechts ist O.K. !";
put CR_
p1=0
p2=0
p3=1
p4=0
pause 3
'steuere hinten_links an, alle anderen aus
print "Strom hinten links:";e_motor_sum_current,
' pause 150
if e_motor_sum_current < unterstrom_einzelmotor OR e_motor_sum_current > ueberstrom_einzelmotor then print "Stromkreis Motor hinten links ist defekt !"; else print "Stromkreis Motor hinten links ist O.K. !";
put CR_
p1=0
p2=0
p3=0
p4=1
pause 3
'steuere hinten_rechts an, alle anderen aus
print "Strom hinten rechts:";e_motor_sum_current,
' pause 10
if e_motor_sum_current < unterstrom_einzelmotor OR e_motor_sum_current > ueberstrom_einzelmotor then print "Stromkreis Motor hinten rechts ist defekt !"; else print "Stromkreis Motor hinten rechts ist O.K. !";
put CR_
p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0
pause 10
return

#hindernis
if hindernis_umfahren = 0 then return
if setting_manuell_fahren = -1 AND (pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle) then gosub stop
if setting_manuell_fahren = 0 AND pts_sensor_left>hindernis_schwelle then gosub hindernis_links_umfahren
if setting_manuell_fahren = 0 AND pts_sensor_right>hindernis_schwelle then gosub hindernis_rechts_umfahren
return

#akku_diagnose
'return ' so lange AD-ports nicht beschaltet, danach return entfernen
akkuzaehler = akkuzaehler +1
if akkudiagnose_ueberwachungs_bit = 0 OR akkuzaehler <> 0 then return
print "<yarp> Antrieb: ";engine_akku_voltage/10;".";engine_akku_voltage mod 10;" Volt ; System: ";control_unit_voltage/10;".";control_unit_voltage mod 10;" Volt ; Antriebstrom: ";((engine_akku_voltage - e_motor_sum_current)-1) * 8;"00 mA";:put CR_
if engine_akku_voltage < unterspannung_e_motoren then gosub engine_akku_voltage_warnmeldung
if control_unit_voltage < unterspannung_control_unit then gosub control_unit_voltage_warnmeldung
return

#engine_akku_voltage_warnmeldung
if diagnose_mode_bit = -1 then print "<yarp>: E-Motoren-Akkuspannung: ";engine_akku_voltage;" Bordnetzspannung: ";control_unit_voltage
print "<yarp>: Alarm ! Antrieb-Unterspannung !";:put CR_
print "<yarp>: Alle Maschinen Stop !"; : put CR_
setting_manuell_fahren = 1
if diagnose_mode_bit = 0 then gosub stop
gosub wait_to_proceed
return

#control_unit_voltage_warnmeldung
print "<yarp>: Alarm ! System-Unterspannung !"; : put CR_
print "<yarp>: Alle Maschinen Stop !"; : put CR_
setting_manuell_fahren = 1
gosub stop
gosub wait_to_proceed
return

#wait_to_proceed
print "<yarp>: Weiter mit <W>";:put CR_
#loop_wait_abfrage_falsche_eingabe
if ser_in_byte <> CR_ then print "?:>"; : put CR_ 'wenn CR_ empfangen wurde, wird kein ? ausgegeben
#loop_wait_abfrage
if RXD then get ser_in_byte else goto loop_wait_abfrage
if ser_in_byte = W_ then ser_in_byte = 250 else goto loop_wait_abfrage_falsche_eingabe
return

#settings
print "<yarp>: Alle Maschinen Stop !";:put CR_
gosub stop

exit_bit = 0 ' das menu wird nicht verlassen, bevor das bit = 1 ist.

#loop_settings_menu
print "Settings Menu:"
print "0. Dieses Menu verlassen (Exit)";:put CR_
print "1. Manuelles Fahren ein/aus : "; : if setting_manuell_fahren = -1 then print "ist ein"; : if setting_manuell_fahren = 0 then print "ist aus";:put CR_
print "2. Diagnose Mode ein/aus : "; : if diagnose_mode_bit = -1 then print "ist ein" : if diagnose_mode_bit = 0 then print "ist aus";:put CR_
print "3. Akku-Ueberwachung ein/aus : "; : if akkudiagnose_ueberwachungs_bit = -1 then print "ist ein" : if akkudiagnose_ueberwachungs_bit = 0 then print "ist aus";:put CR_
print "?:>";: put CR_

#loop_settings_abfrage 'warten bis ein gültiges byte empfangen wurde
if RXD then get ser_in_byte else goto loop_settings_abfrage 'warten bis ein byte empfangen wurde, dann abfrage, ob gültig

if ser_in_byte > 47 and ser_in_byte < 58 then ser_in_byte = ser_in_byte - 48 else goto loop_settings_abfrage
on ser_in_byte gosub s_exit,s_manuell_fahren,s_diagnose_mode,s_akkudiagnose

if exit_bit = 0 then goto loop_settings_menu 'solange dieses bit = 0 wird im settingsmenu geblieben
return

#s_diagnose_mode
diagnose_mode_bit = diagnose_mode_bit = 0
print "<yarp>: ";
if diagnose_mode_bit = -1 then print "Diagnose Mode ist nun aktiv"; else print "Diagnose Mode ist nun inaktiv";
put CR_
return
#s_akkudiagnose
akkudiagnose_ueberwachungs_bit = akkudiagnose_ueberwachungs_bit = 0
print "<yarp>: ";
if akkudiagnose_ueberwachungs_bit = -1 then print "Akkudiagnose ist nun aktiv"; else print "Akkudiagnose ist nun inaktiv";
put CR_

return
#s_exit
print "yarp: ?:>"; : put CR_
exit_bit = 1 ' settingsmenu bei naechster abfrage verlassen
return

#s_manuell_fahren

setting_manuell_fahren = setting_manuell_fahren = 0
print "<yarp>: ";
if setting_manuell_fahren = -1 then print "Manuell Fahren ist nun aktiv"; else print "Manuell Fahren ist nun inaktiv";
put CR_
return

#analyse

return



#TNC_INIT

'wait CTS: put ESC_ : wait CTS:put E_ : put null_ : put CR_
put ESC_:print "E0";:put CR_
put ESC_:print "S0";:put CR_
put ESC_:print "IYARP";:put CR_
put ESC_:print "MN";:put CR_
put ESC_:print "T40";:put CR_
put ESC_:print "@T2150";:put CR_
'put ESC_:print "@T3500";:put CR_
put ESC_:print "O7";:put CR_
put ESC_:print "S1";:put CR_
return

#connect_host
'wait CTS: put ESC_ : put C_ : put D_ : put L_ : put drei_ : put S_ : put J_ : put B_ : put CR_
put ESC_:print "CDL3SJB";:put CR_
return

#hindernis_links_umfahren
if hupe then gosub hupe
gosub rueckwaerts
pause 50
gosub rechts
pause 50
return

#hindernis_rechts_umfahren
if hupe then gosub hupe
gosub rueckwaerts
pause 50
gosub links
pause 50
return


#vorwaerts_ohne_stop
if RXD then input schritt_anzahl
if schritt_anzahl = 0 then schritt_anzahl = 1
'if then schritt_anzahl = 1
p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0

p5=0
p6=0
p7=0
p8=0
p1=1
p2=1
p3=1
p4=1
for zaehler = 1 to schritt_anzahl
pause 5
if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then zaehler = schritt_anzahl
if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then gosub stop
next
if schritt_anzahl > 1 then motoren_port = &H00
schritt_anzahl = 0
return

#vorwaerts_mit_stop

gosub stop
p1=1
p2=1
p3=1
p4=1

return


#rueckwaerts

' gosub stop
if RXD then input schritt_anzahl
if schritt_anzahl = 0 then schritt_anzahl = 1

p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0
p5=1
p6=1
p7=1
p8=1
for zaehler = 1 to schritt_anzahl
pause 5
' if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then zaehler = schritt_anzahl
' if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then gosub stop
next
if schritt_anzahl > 1 then motoren_port = &H00
schritt_anzahl = 0
return

#stop
'if setting_manuell_fahren = 0 AND (pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle) then print "<yarp>: Hindernis während manuellem Fahren erkannt, Stop !";:put CR_
p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0
pause 10
return

#links
' gosub stop
if RXD then input schritt_anzahl
if schritt_anzahl = 0 then schritt_anzahl = 1

p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0
p1=1
p2=0
p3=0
p4=1
p5=1
p6=0
p7=1
p8=0
for zaehler = 1 to schritt_anzahl
pause 5
if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then zaehler = schritt_anzahl
if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then gosub stop
next
if schritt_anzahl > 1 then motoren_port = &H00
schritt_anzahl = 0
return

#rechts

if io_control then if RXD then input schritt_anzahl
if schritt_anzahl = 0 then schritt_anzahl = 1

' gosub stop
p1=0
p2=0
p3=0
p4=0
p5=0
p6=0
p7=0
p8=0

p1=0
p2=1
p3=1
p4=0
p5=0
p6=1
p7=0
p8=1
if io_control then for zaehler = 1 to schritt_anzahl
pause 5
if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then zaehler = schritt_anzahl
if pts_sensor_left>hindernis_schwelle OR pts_sensor_right>hindernis_schwelle then gosub stop
next
if schritt_anzahl > 1 then motoren_port = &H00
schritt_anzahl = 0
return