Hab mich mal an den Algo gesetzt, habe aber im Moment das Problem, dass die rechte Seite immer langsamer wird. Sollte doch nach dem Programm nicht so sein, oder?
Kann sich jemand das mal bitte anschauen?
Code:
'###############################################################################
'Programm zum Geradeausfahren mit Hilfe des Kompass CMPS03
'
'Geht nicht! Warum??
'Wert für Servo_1 wird die ganze Zeit nur erhöht, nicht erniedrigt.
'
'Autor: RN_jon
'Datum: 16.08.2007
'###############################################################################

Declare Function Cmps_himmelsrichtung() As Word

$regfile = "m32def.dat"
$crystal = 16000000
$baud = 19200
$framesize = 42
$swstack = 32
$hwstack = 32

'I2C
Config Scl = Portc.0
Config Sda = Portc.1

'Variablen für Kompass
Dim Himmelsrichtung As Word
Dim Grad As Single

'LEDs
Config Portd.4 = Output                                     'rot
Config Portd.5 = Output                                     'gelb
Config Portd.6 = Output                                     'grün

'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

'Variablen für Geradeausfahrt
Dim Start_cmps As Single

'+++++++++++++++++++++++++

Portd.4 = 1
Portd.5 = 1
Portd.6 = 1

Portd.4 = 0
Waitms 500
Portd.4 = 1
Portd.5 = 0
Waitms 500
Portd.5 = 1
Portd.6 = 0
Waitms 500
Portd.6 = 1

'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Wait 1

Servo_1 = 140
Servo_2 = 140

Himmelsrichtung = Cmps_himmelsrichtung()
Start_cmps = Himmelsrichtung / 10
Print "Startausrichtung: " ; Start_cmps

Do

Servo(1) = Servo_1
Servo(2) = Servo_2

   Himmelsrichtung = Cmps_himmelsrichtung()
   Grad = Himmelsrichtung / 10
   Print "Fahrtausrichtung: " ; Grad

   If Grad < Start_cmps Then                                'nach rechts drehen
      Servo_1 = Servo_1 + 2
   End If
   If Grad > Start_cmps Then                                'nach links drehen
      Servo_1 = Servo_1 - 2
   End If

   Waitms 200
   Print "Servo_1: " ; Servo_1
   Print "Servo_2: " ; Servo_2
Loop

End

'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Function Cmps_himmelsrichtung() As Word
Local Lob As Byte
Local Hib As Byte

   I2cstart
   I2cwbyte &HC0
   I2cwbyte 2
   I2cstop

   I2cstart
   I2cwbyte &HC1
   I2crbyte Hib , Ack
   I2crbyte Lob , Nack
   I2cstop

   Cmps_himmelsrichtung = Makeint(lob , Hib)
End Function
jon

P.S.: Es ist im Moment so, dass es nicht funktionieren würde, wenn der Bot über die 360° fährt. Wenn er also meinetwegen bei 359° startet und dann nach 1° abdriftet. Dass habe ich im Moment noch nicht in ein Programm umgesetzt.
Ich habe es nur so getestet, dass ich den Bot vor dem Programmstart nach Süden ausgerichtet habe.