Programm stoppt in einer Funktion - Wo ist mein Fehler
Hallo,
ich sitze jetzt schon seit heute morgen an diesem Problem ](*,) :
Ich habe diese Code:
Code:
Declare Function Twi_read_byte(byval Slave As Byte) As Byte
Declare Function Srf02_entfernung(byval Slaveid As Byte) As Integer
$regfile = "M32def.dat" ' the used chip
$crystal = 16000000 ' frequency used
$framesize = 84
$swstack = 84
$hwstack = 84
$baud = 19200 ' baud rate
Config Scl = Portc.0
Config Sda = Portc.1
I2cinit
Const Srf02_slaveid = &HE0
Dim Entfernung_hi As Integer
Dim Gegenstand_hi As Byte
Dim Gegenstand_alle As Byte
Dim Twi_control As Byte ' Controlregister lokale kopie
Dim Twi_status As Byte
Dim Twi_data As Byte
Dim Eingang_slave_senoren As Byte
Dim Eingang_slave_led As Byte
Dim Error As Byte ' Fehlermerker
' TWI Init
Twcr = &B00000100 ' erstmal nur TWI aktivieren
Twsr = 0 ' Status und Prescaler Register
Twbr = 72 ' Bit Rate Register, 100kHz
'Motoren:
Config Servos = 2 , Servo1 = Portc.2 , Servo2 = Portc.3 , Reload = 5
Config Portc.2 = Output 'rechts
Config Portc.3 = Output 'links
Enable Interrupts
Dim Servo_1 As Byte
Dim Servo_2 As Byte
'192 = Stopp
'128 = Vollgas vor
'255 = Vollgas rück
'Main ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Wait 2
Do
'Entfernung_hi = Srf02_entfernung(srf02_slaveid)
'If Entfernung_hi < 40 Then
' Gegenstand_hi = 4
' Else
' Gegenstand_hi = 0
'End If
Gegenstand_hi = 0
Eingang_slave_senoren = Twi_read_byte(&H40)
Print Eingang_slave_senoren ;
Gegenstand_alle = Eingang_slave_senoren + Gegenstand_hi
Select Case Gegenstand_alle
Case 0 : Gosub Vorm
Case 1 : Gosub Rechtsm
Case 2 : Gosub Linksm
Case 3 : Gosub Rueckm
Case 4 : Gosub Vorm
Case 5 : Gosub Rechtsm
Case 6 : Gosub Linksm
Case 7 : Gosub Stopm
End Select
Waitms 100
Loop
End
'Subs ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
'------------- Empfangen von Slave_sensoren @ &H40 -----------------------------
Function Twi_read_byte(slave As Byte) As Byte
Error = 0
Twi_read_byte = 0
Twcr = &B10100100
Gosub Twi_wait_int
If Twi_status = &H08 Or Twi_status = &H10 Then
Twdr = Slave Or &H01 ' slave adresse + Read
Twcr = &B10000100 ' TWINT löschen, Byte senden
Gosub Twi_wait_int
If Twi_status = &H40 Then
Twcr = &B10000100
Gosub Twi_wait_int
If Twi_status = &H58 Or Twi_status = &H50 Then
Twi_read_byte = Twdr ' Daten lesen
Error = 0 ' kein Fehler
Else
Error = Twi_status ' Fehler
End If
Else
Error = Twi_status ' Fehler
End If
Twcr = &B10010100
Else
Twcr = &B10000100 ' TWINT löschen, Bus freigeben
Error = Twi_status ' Fehler
End If
End Function
Twi_wait_int:
Do
Twi_control = Twcr And &H80
Loop Until Twi_control = &H80
Twi_status = Twsr And &HF8
Return
'------------- SRF02 (hinten) --------------------------------------------------
Function Srf02_entfernung(byval Slaveid As Byte) As Integer
Local Lob As Byte
Local Hib As Byte
Local Slaveid_read As Byte
Slaveid_read = Slaveid + 1
I2cstart
I2cwbyte Slaveid
I2cwbyte 0
I2cwbyte 81 'in Zentimetern messen
I2cstop
I2cstart
I2cwbyte Slaveid
I2cwbyte 2 'Leseregister festlegen
I2cstop
I2cstart
I2cwbyte Slaveid_read
I2crbyte Hib , Ack
I2crbyte Lob , Nack
I2cstop
Srf02_entfernung = Makeint(lob , Hib)
End Function
End
'------------- Motorbefehle ----------------------------------------------------
Vorm:
Servo(1) = 150
Servo(2) = 150
Return
Linksm:
Servo(1) = 225
Servo(2) = 158
Return
Rechtsm:
Servo(1) = 158
Servo(2) = 225
Return
Stopm:
Servo(1) = 192
Servo(2) = 192
Return
Rueckm:
Servo(1) = 230
Servo(2) = 230
Return
Die ersten paar Sekunden läuft das auch so, wie es soll. Aber danach endet er in der Funktion Twi_read_byte, die ich aus der Wiki habe. Die Abfrage des SRF02 habe ich im Moment auskommentiert, da ich damit weitere Fehlerquelle vermeiden möchte, und weitere Geräte am I2C-bus gibt es nicht.
Kann mir jemand weiterhelfen, oder braucht ihr noch irgendwelche Infos?
Ich bin dankbar für jede Antwort, da ich das für den Robotiktreff in Braunschweig brauche!
jon
