Code:
Declare Function Tastenabfrage() As Byte
Declare Sub Vorwaerts()
Declare Sub Rueckwaerts()
Declare Sub Rechts()
Declare Sub Links()
Const Sd21_slaveid = &HC2
Declare Function Sd21_version() As Byte
Declare Sub Setzeservoposition(byval Servo As Byte , Byval Position As Word)
Declare Sub Setzeservospeed(byval Servo As Byte , Byval Speed As Byte)
$regfile = "m32def.dat"
$framesize = 42
$swstack = 42
$hwstack = 42
$crystal = 16000000 'Quarzfrequenz
$baud = 9600
Config Scl = Portc.0 'Ports fuer IIC-Bus
Config Sda = Portc.1
Config Pina.7 = Input 'Für Tastenabfrage
Porta.7 = 1 'Pullup Widerstand ein
Dim I As Integer
Dim Taste As Byte
Wait 2 'Warte 2 Sekunden
I2cinit
Setzeservospeed 1 , 255
Setzeservospeed 15 , 255
Setzeservospeed 2 , 255
Setzeservospeed 21 , 255
Setzeservospeed 6 , 255
Setzeservospeed 20 , 255
Setzeservospeed 4 , 25
Setzeservospeed 13 , 25
Setzeservospeed 10 , 25
Setzeservospeed 17 , 25
Setzeservospeed 8 , 25
Setzeservospeed 11 , 25
Setzeservoposition 4 , 1500 'Drehservos Grundstellung
Setzeservoposition 13 , 1500
Setzeservoposition 10 , 1500
Setzeservoposition 17 , 1500
Setzeservoposition 8 , 1500
Setzeservoposition 11 , 1500
Setzeservoposition 1 , 1000 'Beinheber Grundstellung
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
Waitms 100
Do
Taste = Tastenabfrage()
If Taste <> 0 Then
Select Case Taste
Case 1
Call Vorwaerts
Case 2
Call Rechts
Case 3
Call Rueckwaerts
Case 4
Call Links
End Select
End If
Waitms 100
Loop
End
Function Sd21_version() As Byte
Local Firmware As Byte
Local I2cread As Byte
I2cread = Sd21_slaveid + 1
I2cstart
I2cwbyte Sd21_slaveid
I2cwbyte 64 'Leseregister festlegen
I2cstop
I2cstart
I2cwbyte I2cread
I2crbyte Firmware , Nack
I2cstop
Sd21_version = Firmware
End Function
'Die Position wird gewöhnlich von 1000 bis 2000 angegeben
'Manche Servos haben etwas größeren Drehbereich, mit etwas
'niedrigeren und höheren Werten kann man diesen auch nutzen
Sub Setzeservoposition(byval Servo As Byte , Byval Position As Word)
Local Register As Byte
Local Lowpos As Byte
Local Highpos As Byte
Register = Servo - 1
Register = Register * 3
Register = Register + 1
Lowpos = Low(position)
Highpos = High(position)
I2cstart
I2cwbyte Sd21_slaveid
I2cwbyte Register 'Register
I2cwbyte Lowpos
I2cwbyte Highpos
I2cstop
End Sub
'Setze Geschwindigkeit der Drehbewegung (1 ganz langsam 255=ganz schnell)
Sub Setzeservospeed(byval Servo As Byte , Byval Speed As Byte)
Local Register As Byte
Register = Servo - 1
Register = Register * 3
I2cstart
I2cwbyte Sd21_slaveid
I2cwbyte Register 'Register
I2cwbyte Speed
I2cstop
End Sub
' Diese Unterfunktion fragt die Tastatur am analogen Port ab
' Sollte beim betätigen einer Taste kein Quittungston kommen, dann
' muss die die Tastenabfrage (Select Case Anweisung in Funktion )
' an ihr Board angepaßt werden. Widerstandstoleranzen sorgen in
' Einzelfällen manchmal dafür das die Werte etwas anders ausfallen
' Am besten dann den WS wert mit Print für jede Taste ausgeben lassen
Function Tastenabfrage() As Byte
Local Ws As Word
Tastenabfrage = 0
Ws = Getadc(7)
' Print "ws= " ; Ws
If Ws < 1010 Then
Select Case Ws
Case 400 To 455
Tastenabfrage = 1
Case 335 To 380
Tastenabfrage = 2
Case 250 To 305
Tastenabfrage = 3
Case 180 To 220
Tastenabfrage = 4
Case 100 To 130
Tastenabfrage = 5
End Select
End If
End Function
Sub Vorwaerts()
For I = 1 To 6
Setzeservoposition 1 , 2000 'Vorwärts laufen
Setzeservoposition 15 , 1000
Setzeservoposition 2 , 2000
Setzeservoposition 4 , 1200
Setzeservoposition 13 , 1800
Setzeservoposition 10 , 1200
Setzeservoposition 17 , 1200
Setzeservoposition 8 , 1800
Setzeservoposition 11 , 1200
Waitms 500
Setzeservoposition 1 , 1000
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Waitms 150
Setzeservoposition 21 , 1000
Setzeservoposition 6 , 2000
Setzeservoposition 20 , 1000
Setzeservoposition 4 , 1800
Setzeservoposition 13 , 1200
Setzeservoposition 10 , 1800
Setzeservoposition 17 , 1800
Setzeservoposition 8 , 1200
Setzeservoposition 11 , 1800
Waitms 500
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
Waitms 150
Next I
Setzeservoposition 4 , 1500 'Drehservos Grundstellung
Setzeservoposition 13 , 1500
Setzeservoposition 10 , 1500
Setzeservoposition 17 , 1500
Setzeservoposition 8 , 1500
Setzeservoposition 11 , 1500
Setzeservoposition 1 , 1000 'Beinheber Grundstellung
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
End Sub
Sub Rechts()
For I = 1 To 5
Setzeservoposition 1 , 2000 'Drehung nach Rechts
Setzeservoposition 15 , 1000
Setzeservoposition 2 , 2000
Setzeservoposition 4 , 1000
Setzeservoposition 13 , 1000
Setzeservoposition 10 , 1000
Setzeservoposition 17 , 2000
Setzeservoposition 8 , 2000
Setzeservoposition 11 , 2000
Waitms 500
Setzeservoposition 1 , 1000
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Waitms 150
Setzeservoposition 21 , 1000
Setzeservoposition 6 , 2000
Setzeservoposition 20 , 1000
Setzeservoposition 4 , 2000
Setzeservoposition 13 , 2000
Setzeservoposition 10 , 2000
Setzeservoposition 17 , 1000
Setzeservoposition 8 , 1000
Setzeservoposition 11 , 1000
Waitms 500
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
Waitms 150
Next I
Setzeservoposition 4 , 1500 'Drehservos Grundstellung
Setzeservoposition 13 , 1500
Setzeservoposition 10 , 1500
Setzeservoposition 17 , 1500
Setzeservoposition 8 , 1500
Setzeservoposition 11 , 1500
Setzeservoposition 1 , 1000 'Beinheber Grundstellung
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
End Sub
Sub Rueckwaerts()
For I = 1 To 6
Setzeservoposition 1 , 2000 'Rückwärts laufen
Setzeservoposition 15 , 1000
Setzeservoposition 2 , 2000
Setzeservoposition 4 , 1800
Setzeservoposition 13 , 1200
Setzeservoposition 10 , 1800
Setzeservoposition 17 , 1800
Setzeservoposition 8 , 1200
Setzeservoposition 11 , 1800
Waitms 500
Setzeservoposition 1 , 1000
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Waitms 150
Setzeservoposition 21 , 1000
Setzeservoposition 6 , 2000
Setzeservoposition 20 , 1000
Setzeservoposition 4 , 1200
Setzeservoposition 13 , 1800
Setzeservoposition 10 , 1200
Setzeservoposition 17 , 1200
Setzeservoposition 8 , 1800
Setzeservoposition 11 , 1200
Waitms 500
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
Waitms 150
Next I
Setzeservoposition 4 , 1500 'Drehservos Grundstellung
Setzeservoposition 13 , 1500
Setzeservoposition 10 , 1500
Setzeservoposition 17 , 1500
Setzeservoposition 8 , 1500
Setzeservoposition 11 , 1500
Setzeservoposition 1 , 1000 'Beinheber Grundstellung
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
End Sub
Sub Links()
For I = 1 To 5
Setzeservoposition 1 , 2000 'Drehung nach Links
Setzeservoposition 15 , 1000
Setzeservoposition 2 , 2000
Setzeservoposition 4 , 2000
Setzeservoposition 13 , 2000
Setzeservoposition 10 , 2000
Setzeservoposition 17 , 1000
Setzeservoposition 8 , 1000
Setzeservoposition 11 , 1000
Waitms 500
Setzeservoposition 1 , 1000
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Waitms 150
Setzeservoposition 21 , 1000
Setzeservoposition 6 , 2000
Setzeservoposition 20 , 1000
Setzeservoposition 4 , 1000
Setzeservoposition 13 , 1000
Setzeservoposition 10 , 1000
Setzeservoposition 17 , 2000
Setzeservoposition 8 , 2000
Setzeservoposition 11 , 2000
Waitms 500
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
Waitms 150
Next I
Setzeservoposition 4 , 1500 'Drehservos Grundstellung
Setzeservoposition 13 , 1500
Setzeservoposition 10 , 1500
Setzeservoposition 17 , 1500
Setzeservoposition 8 , 1500
Setzeservoposition 11 , 1500
Setzeservoposition 1 , 1000 'Beinheber Grundstellung
Setzeservoposition 15 , 2000
Setzeservoposition 2 , 1000
Setzeservoposition 21 , 2000
Setzeservoposition 6 , 1000
Setzeservoposition 20 , 2000
End Sub
Bitte um Hilfe was ich hier falsch gemacht habe.
Zitieren
Lesezeichen