' Testprogramm von TESA, geschrieben in Bascom Basic von
' Michael Kinz
' www.team-iwan.de
$crystal = 16000000
$regfile = "m8def.dat"
Config Portd = Output
Config Portb = Output
Config Scl = Portc.5
Config Sda = Portc.4
Config Lcdpin = Pin , Db4 = Portd.4 , Db5 = Portd.5 , Db6 = Portd.6 , Db7 = Portd.7 , E = Portd.3 , Rs = Portd.2
Config Lcd = 20 * 2

Deflcdchar 0 , 4 , 14 , 31 , 32 , 32 , 4 , 14 , 31          ' replace ? with number (0-7)

Deflcdchar 1 , 31 , 14 , 4 , 32 , 32 , 31 , 14 , 4          ' replace ? with number (0-7)

Portc.3 = 1                                                 'Schalter
Portc.2 = 1                                                 'Schalter2

Const Adr_sli_w = 20
Const Adr_sli_r = 21
Const Adr_sre_w = 22
Const Adr_sre_r = 23
Const Adr_mli_w = 4
Const Adr_mli_r = 5
Const Adr_mre_w = 6
Const Adr_mre_r = 7

Dim Seitwl As Word
Dim Seitwlalt As Word
Dim Seitl As Byte
Dim Vornwl As Word
Dim Vornwlalt As Word
Dim Vornl As Byte
Dim Seitwr As Word
Dim Seitwralt As Word
Dim Seitr As Byte
Dim Vornwr As Word
Dim Vornwralt As Word
Dim Vornr As Byte
Dim Tempw As Word
Dim Temp As Byte
Dim Tempi As Integer
'Dim Zaehler As Word
Dim Hilfsbyte As Byte
Dim Debugg As Byte
Hilfsbit Alias Hilfsbyte.0
M_enable Alias Hilfsbyte.1
Led Alias Portb.5
Dim Z As Word
Dim Bla As Byte
Dim X As Byte
Dim Wechsel As Byte
Dim We_z As Byte
Dim Gleich As Byte
Dim I As Byte
Dim Pwml As Byte
Dim Pwmr As Byte
Dim Dirl As Byte
Dim Dirr As Byte

Dim Sharp(4) As Byte

 Dim Ergebnis(10) As Byte

 Dim Driftl As Integer
 Dim Driftr As Integer
 Dim Driftv As Integer
 Dim Driftz As Integer
 Dim Richtung As Integer
 Dim Speed As Integer

 Dim Mlinks As Integer
 Dim Mrechts As Integer

Dim Speedmax As Integer
Dim Abstand As Byte












Cls
Cursor Off
Locate 1 , 2
Lcd "TEAM IWAN presents"
Waitms 500

Cls
Locate 1 , 9
Lcd "TESA   V.03"
Waitms 500

I2cstart
I2cwbyte Adr_sli_w
I2cwbyte 0
I2cwbyte 12
I2cstop

I2cstart
I2cwbyte Adr_sre_w
I2cwbyte 0
I2cwbyte 12
I2cstop


'Motor links init:
I2cstart
I2cwbyte Adr_mli_w
I2cwbyte 0
I2cwbyte 131
I2cwbyte 0
I2cwbyte 2
I2cstop

'Motor rechts init:
I2cstart
I2cwbyte Adr_mre_w
I2cwbyte 0
I2cwbyte 131
I2cwbyte 0
I2cwbyte 2
I2cstop

Cls

Debugg = 0
Speedmax = 70
Abstand = 27
'------------------- Hauptschleife ----------------------
Do
'Waitms 100

If Pinc.3 = 0 Then
M_enable = 0
Incr Debugg
If Debugg > 6 Then
Debugg = 0
End If
Cls
Lcd Debugg
Bitwait Pinc.3 , Set
Cls
End If


Gosub Leselinks

Gosub Leserechts


Gosub Rechnen






Gosub Richtung


'Pwmr = Pwml
If Debugg = 0 Then
Gosub Speedeinstellung
End If

If Debugg = 6 Then
Gosub Abstandeinstellung
End If
If Debugg = 5 Then
M_enable = 1
End If


If M_enable = 1 Then
Else
Dirl = Dirl And 3
Dirr = Dirr And 3
End If


Gosub Motorlinks

Gosub Motorrechts

Toggle Led
Loop

End
' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Richtung:
'If Pwml > 50 Then
'     Driftz = 0
'End If
'If Pwmr > 50 Then
Driftz = 0

'End If

Driftv = 0
Driftr = 0
Driftl = 0
If Seitl > 2 Then
  If Seitl <= Abstand Then
   Driftr = Abstand - Seitl
  End If
End If
If Seitr > 2 Then
  If Seitr <= Abstand Then
   Driftl = Abstand - Seitr
  End If
End If
If Vornl > 2 Then
Tempi = Abstand + 7
 If Vornl <= Tempi Then
  Tempi = Tempi - Vornl
  Driftz = Driftz + Tempi
  Driftr = Driftr + Tempi
 Else

 End If
End If
If Vornr > 2 Then
    Tempi = Abstand + 7
 If Vornr <= Tempi Then
  Tempi = Tempi - Vornr
  Driftz = Driftz + Tempi
  Driftl = Driftl + Tempi
 End If
End If

If Debugg = 3 Then
 Gosub Driftanzeige
End If

Richtung = Driftr - Driftl
Tempi = Driftr + Driftl
Tempi = Tempi / 2
Driftz = Driftz + Tempi
Speed = Speedmax - Driftz

Richtung = Richtung * 4
Speed = Speed * 2

Mlinks = Richtung + Speed

Mrechts = Speed - Richtung


If Mlinks < 0 Then
  Gosub Mlr
 Else
  Gosub Mlv
End If

Mlinks = Abs(mlinks)



If Mrechts < 0 Then
  Gosub Mrr
 Else
  Gosub Mrv
End If
Mrechts = Abs(mrechts)



Do
Tempw = Mrechts + Mlinks

Incr Mrechts
Incr Mlinks

Loop Until Tempw > 255

If Mlinks > 255 Then
 Mlinks = 255
End If
Pwml = Mlinks

If Mrechts > 255 Then
 Mrechts = 255
End If
Pwmr = Mrechts









If Debugg = 4 Then
Gosub Richtungsanzeige
End If




Return
'-----------------------------------------------------------
'-----------------------  Rechnen  -------------------------
'-----------------------------------------------------------
Rechnen:
Seitl = Seitwl / 10
Seitr = Seitwr / 10
Vornl = Vornwl / 10
Vornr = Vornwr / 10






If Debugg = 2 Then
 Gosub Zentimeter
End If




Return
Zentimeter:
Locate 1 , 1
Lcd "l:" ; Driftl ; " "
Locate 1 , 6
Lcd "r:" ; Driftr ; " "
Locate 1 , 11
Lcd "v:" ; Driftv ; " "
Locate 1 , 16
Lcd "z:" ; Driftz ; " "
Return
Driftanzeige:
Locate 1 , 1
Lcd "l:" ; Seitl ; " "
Locate 1 , 6
Lcd "v:" ; Vornl ; " "
Locate 1 , 11
Lcd "v:" ; Vornr ; " "
Locate 1 , 16
Lcd "r:" ; Seitr ; " "
Return
Richtungsanzeige:
Locate 1 , 1
Lcd "Rchtg:" ; Richtung ; "  "
Locate 1 , 10
Lcd "Speed: " ; Speed ; "   "
Return
'----------------------- Motor LINKS ----------------------------

Motorlinks:

I2cstart
I2cwbyte Adr_mli_w
I2cwbyte 0
I2cwbyte Dirl
I2cwbyte Pwml
I2cstop



Return
'------------------------ M Rechts -------------------------
Motorrechts:

I2cstart
I2cwbyte Adr_mre_w
I2cwbyte 0
I2cwbyte Dirr
I2cwbyte Pwmr
I2cstop



Return

'----------------------------------------
Leselinks:
I2cstart
I2cwbyte Adr_sli_w
I2cwbyte 0

I2cstart
I2cwbyte Adr_sli_r

I2crbyte Ergebnis(1) , Ack
I2crbyte Ergebnis(2) , Ack
I2crbyte Ergebnis(3) , Ack
I2crbyte Ergebnis(4) , Ack
I2crbyte Ergebnis(5) , Nack
I2cstop

Seitwl = Ergebnis(1) * 256
Seitwl = Seitwl + Ergebnis(2)
Vornwl = Ergebnis(3) * 256
Vornwl = Vornwl + Ergebnis(4)

If Seitwl > 70 Then
   Seitwl = Seitwl - 70
   Seitwl = 24000 / Seitwl
   Seitwlalt = Seitwl
   Else
   Seitwl = Seitwlalt

   End If
If Debugg = 1 Then
Locate 1 , 1
Lcd ":" ; Seitwl ; "  "
End If
If Vornwl > 21 Then
   Vornwl = Vornwl - 21
   Vornwl = 28533 / Vornwl
   Vornwlalt = Vornwl
   Else
   Vornwl = Vornwlalt

   End If

If Debugg = 1 Then
Locate 1 , 6
Lcd ":" ; Vornwl ; "  "
End If


Return
'-----------------------------------------------------
Leserechts:
I2cstart
I2cwbyte Adr_sre_w
I2cwbyte 0

I2cstart
I2cwbyte Adr_sre_r

I2crbyte Ergebnis(1) , Ack
I2crbyte Ergebnis(2) , Ack
I2crbyte Ergebnis(3) , Ack
I2crbyte Ergebnis(4) , Ack
I2crbyte Ergebnis(5) , Nack
I2cstop





Seitwr = Ergebnis(1) * 256
Seitwr = Seitwr + Ergebnis(2)
Vornwr = Ergebnis(3) * 256
Vornwr = Vornwr + Ergebnis(4)

If Vornwr > 11 Then
   Vornwr = Vornwr - 11
   Vornwr = 28533 / Vornwr
   Vornwralt = Vornwr
   Else
   Vornwr = Vornwralt

   End If
If Debugg = 1 Then
Locate 1 , 11
Lcd ":" ; Vornwr ; "  "
End If

If Seitwr > 13 Then
   Seitwr = Seitwr - 13
   Seitwr = 27200 / Seitwr
   Seitwralt = Seitwr
   Else
   Seitwr = Seitwralt

   End If

If Debugg = 1 Then
Locate 1 , 16
Lcd ":" ; Seitwr ; "  "
End If

Return
'--------------------------------------------------------

Vorw:
Dirr = &B01000011
Dirl = &B01000011
Gosub Deb3
Return
Rueckw:
Dirr = &B10000011
Dirl = &B10000011
If Debugg = 3 Then
Locate 1 , 7
Lcd Chr(1) ; " " ; Chr(1)
End If
Return
Rechts:
Dirr = &B01000011
Dirl = &B10000011
If Debugg = 3 Then
Locate 1 , 7
Lcd Chr(0) ; " " ; Chr(1)
End If
Return
Links:
Dirr = &B10000011
Dirl = &B01000011
If Debugg = 3 Then
Locate 1 , 7
Lcd Chr(1) ; " " ; Chr(0)
End If
Return

Deb3:
If Debugg = 3 Then
Locate 1 , 7
Lcd Chr(0) ; " " ; Chr(0)
End If
Return

Mlv:
Dirl = &B01000011
Return
Mlr:
Dirl = &B10000011
Return
Mrv:
Dirr = &B01000011
Return
Mrr:
Dirr = &B10000011
Return
Speedeinstellung:



Locate 1 , 1
Lcd "Speed max. " ; Speedmax ; " "

If Pinc.2 = 0 Then
Bitwait Pinc.2 , Set
Speedmax = Speedmax + 5
End If
If Speedmax > 100 Then
Speedmax = 50
End If

Return

Abstandeinstellung:



Locate 1 , 1
Lcd "Abstand " ; Abstand ; " "

If Pinc.2 = 0 Then
Bitwait Pinc.2 , Set
Abstand = Abstand + 1
End If
If Abstand > 40 Then
Abstand = 20
End If

Return