Hallo hier mal ein denkanstoß,dieses Programm ist noch nicht fertig,
und noch nicht dokomentiert,da ich nicht glaube das es im endefekt so bleibt.
Bin selber erst in Roboter steuerung eingestiegen.
Print deshalb weil ein Atmega 8 die Motorsteuerung macht.
Ich hoffe das es Dir hilft.
Gruß raggy
Code:
$regfile = "M32DEF.DAT"

$baud = 19200

$hwstack = 288

$swstack = 288

$framesize = 288

$crystal = 16000000                                         '7372800                                          ' it is neceserry to use an external crystal with excectly these frequency



$lib "S108_KL.lbx"



Config Graphlcd = 128 * 64sed , Dataport = Portc , Controlport = Portb , Ce = 0 , Ce2 = 1 , Cd = 4 , Rd = 3 , Reset = 2 , Enable = 5

Cls

Wait 2

Setfont Font8x8

Wait 1

Lcdat 1 , 25 , "BALOU II"



Config Servos = 1 , Servo1 = Portd.7 , Reload = 10







Config Adc = Single , Prescaler = Auto , Reference = Avcc

'Start Adc

 Enable Interrupts

 Servo(1) = 80

Config Portd.7 = Output

Config Portd.2 = Output





Declare Sub Servo_ultra()

Declare Sub Messen()







Dim I As Byte

Dim Mitte As Word

Dim H_links As Word

Dim H_rechts As Word

Dim Links As Word

Dim Rechts As Word

Dim Zeitmessung As Word

Dim Entfernung As Word



Dim Befehl As String * 6





Portd.2 = 0

Portd.7 = 0

Wait 2



Servo(1) = 120



Wait 15



Do

  Call Servo_ultra()





Loop

'*******************************************************************************

'*******************************************************************************

Sub Servo_ultra()

    For I = 80 To 160

     Servo(1) = I

     If I = 80 Then Call Messen()

     If I = 100 Then Call Messen()

     If I = 120 Then Call Messen()

     If I = 140 Then Call Messen()

     If I = 160 Then Call Messen()

     Waitms 8

     Next I

        For I = 160 To 80 Step -1

     Servo(1) = I

     If I = 100 Then Call Messen()

     If I = 120 Then Call Messen()

     If I = 140 Then Call Messen()

     Waitms 8

     Next I

End Sub

'*******************************************************************************

Sub Messen()

      Pulseout Portd , 2 , 40

      Pulsein Zeitmessung , Pind , 2 , 1

      Zeitmessung = Zeitmessung * 10                        'mal 10 da Pulsein in 10uS

      'Einheiten Ergebnis Ermittelt

      Entfernung = Zeitmessung / 58

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

      If I = 80 Or I = 100 And Entfernung < 18 Then Print "M_Rech";



      If I = 120 And Entfernung > 25 Then Print "M_Grad";

      If I = 120 And Entfernung > 40 Then Print "M_Schn";

      If I = 120 And Entfernung > 30 Then Print "M_Mite";



      If I = 140 Or I = 160 And Entfernung < 18 Then Print "M_Link";



      If Entfernung < 10 Then Print "M_Ruec";

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

   End Sub

'*******************************************************************************

$include "font8x8.font"