![-=[Cassiopeia]=- ist offline](images/statusicon/user-offline.png)
gut du hast es nicht anders gewollt...
hier ist alles was ich habe...:
Code:Declare Sub Taste1() Declare Sub Taste2() Declare Sub Taste3() Declare Sub Taste4() Declare Sub Taste5() Declare Sub Move() Declare Sub Turn180() Declare Sub Stopnow() Declare Sub Anfahren() Declare Sub Turnleft() Declare Sub Turnright() Declare Sub Look() Declare Sub Bremsen() Declare Sub Rewind() Declare Sub Lcdout() Declare Sub Batteriespannung() Declare Sub Temp() Declare Function Tastenabfrage() As Byte $regfile = "m32def.dat" $crystal = 16000000 $baud = 9600 Config Adc = Single , Prescaler = Auto Const Ref = 5 / 1023 Const Center = 109 Dim Ton As Integer Dim Taste As Byte Dim Volt As Single Dim Dist(12) As Integer Dim Servopos As Integer Dim V As Integer Dim Result As Byte Dim Ms As Integer Dim A As Byte Dim Name As String * 20 Name = "RP 005" Dim Text2 As String * 20 Dim Text3 As String * 20 Dim Dl As Integer Dl = 1 'Ports für linken Motor Config Pinc.6 = Output 'Linker Motor Kanal 1 Config Pinc.7 = Output 'Linker Motor Kanal 2 Config Pind.4 = Output 'Linker Motor PWM 'Ports für rechten Motor Config Pinb.0 = Output 'Rechter Motor Kanal 1 Config Pinb.1 = Output 'Rechter Motor Kanal 2 Config Pind.5 = Output 'Rechter Motor PWM Config Timer1 = Pwm , Pwm = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down Pwm1a = 0 Pwm1b = 0 Tccr1b = Tccr1b Or &H02 'ende motorports 'In out für tasten Config Adc = Single , Prescaler = Auto 'Für Tastenabfrage und Spannungsmessung Config Pina.7 = Input 'Für Tastenabfrage Porta.7 = 1 'Pullup Widerstand ein 'ende in out für tasten 'timer config Config Timer1 = Timer , Prescale = 1024 On Timer1 Timer_irq Const Timervorgabe = 3036 Enable Timer1 'timer config 'servo config Config Servos = 1 , Servo1 = Porta.0 , Reload = 10 Config Pina.0 = Output 'ende servo config 'taster config Config Pind.6 = Input Portd.6 = 1 'lcd config Config Lcdpin = Pin , Db4 = Portc.2 , Db5 = Portc.3 , Db6 = Portc.4 , Db7 = Portc.5 , E = Portb.4 , Rs = Portb.3 Config Lcd = 20 * 4 'i2c config Config Scl = Portc.0 Config Sda = Portc.1 Dim Device As Byte Dim Deviceread As Byte Dim Lowtemp As Byte Dim Hightemp As Byte Dim Value As Single I2cinit Device = 144 Deviceread = 145 'ende I2C temp config Enable Interrupts 'programm start Servopos = Center Servo(1) = Servopos Print "**** RN-Control ist startklar... Los gehts!!! *****" Sound Portd.7 , 400 , 450 'BEEP Waitms 100 Sound Portd.7 , 400 , 250 Waitms 100 'BEEP Sound Portd.7 , 400 , 450 'BEEP Cursor Off Text2 = "I am alive..." Call Lcdout Wait 1 Text2 = "Erwarte Befehle" Call Lcdout Do Taste = Tastenabfrage() If Taste <> 0 Then Select Case Taste Case 1 Call Taste1 'Taste 1 Servos Case 2 Call Taste2 'Taste 2 Spannung an pin A5 Case 3 Call Taste3 'Taste 3 Case 4 Call Taste4 'Taste 4 Case 5 Call Taste5 'Taste 5 End Select Sound Portd.7 , 400 , 500 'BEEP End If Waitms 100 Loop End Function Tastenabfrage() As Byte Local Ws As Word Tastenabfrage = 0 Ton = 600 Start Adc Ws = Getadc(7) If Ws < 500 Then Select Case Ws Case 400 To 450 Tastenabfrage = 1 Ton = 550 Case 330 To 380 Tastenabfrage = 2 Ton = 500 Case 260 To 305 Tastenabfrage = 3 Ton = 450 Case 180 To 220 Tastenabfrage = 4 Ton = 400 Case 90 To 130 Tastenabfrage = 5 Ton = 350 End Select Sound Portd.7 , 400 , Ton 'BEEP End If End Function Sub Taste1() Text2 = "Servo Test" Call Lcdout() Wait 1 Servopos = Center Servo(1) = Servopos Text2 = "Mitte" Call Lcdout() Wait 1 Servopos = 60 Servo(1) = Servopos Wait 1 Text2 = "Rechtsdrehung" Call Lcdout() For Servopos = 60 To 160 Step 10 Servo(1) = Servopos Waitms 10 Next Wait 1 Text2 = "Linksdrehung" Call Lcdout() For Servopos = 160 To 60 Step -10 Servo(1) = Servopos Waitms 10 Next Wait 1 Text2 = "Mitte" Call Lcdout() Servopos = Center Servo(1) = Servopos End Sub Sub Taste2() Text2 = "Sensor Test" Call Lcdout() Call Look() End Sub Sub Taste3() Text2 = "Taster Test" Call Lcdout() Do If Pind.6 = 0 Then Sound Portd.7 , 800 , 800 Text2 = "Upps sorry..." Call Lcdout() Call Turn180 End If Waitms 300 Loop End Sub Sub Taste4() Wait 1 Call Turn180 End Sub Sub Taste5() Text2 = "Volle Fahrt voraus" Call Lcdout() Wait 1 Call Move() End Sub Sub Move() Text2 = "moving..." Call Lcdout() Local U As Integer Start Adc 'spannungsmessung starten Call Anfahren 'anfahren 'Fahren Do 'servo Rechtsdrehung" Servopos = 89 For Servopos = 89 To 129 Step 4 Servo(1) = Servopos If Pind.6 = 0 Then 'taster anstoss Text2 = "Upps sorry..." Call Lcdout() Call Stopnow Call Rewind Call Turn180 Call Anfahren Text2 = "moving..." Call Lcdout() End If Waitms 80 'vorher 80 U = Getadc(5) If U > 250 Then 'spannung am gp12d2 zu groß Text2 = "Hindernis gefunden" Call Lcdout() U = 0 Call Stopnow Call Look Call Anfahren Text2 = "moving..." Call Lcdout() End If Next 'servo Linksdrehung" For Servopos = 129 To 89 Step -4 Servo(1) = Servopos If Pind.6 = 0 Then Text2 = "Upps sorry..." Call Lcdout() Call Stopnow Call Rewind Call Turn180 Call Anfahren Text2 = "moving..." Call Lcdout() End If Waitms 80 'vorher 80ms U = Getadc(5) If U > 250 Then Text2 = "Hindernis gefunden" Call Lcdout() U = 0 Call Stopnow Call Look Call Anfahren Text2 = "moving..." Call Lcdout() End If Next Loop 'bremsen If V > 0 Then Call Bremsen End If Servopos = Center Servo(1) = Servopos End Sub Sub Look() Local Sp As Byte Servopos = 60 Servo(1) = Servopos Cls 'clear the LCD display Text2 = "Suche weiteren Weg..." Call Lcdout() Waitms 200 Sp = 1 'Servo "Rechtsdrehung": For Servopos = 50 To 160 Step 10 'vorher 170 Servo(1) = Servopos Waitms 180 Dist(sp) = Getadc(5) Incr Sp Next Servopos = Center Servo(1) = Servopos 'suche nach kleinstem wert Result = 1 For Sp = 1 To 12 'vorher 12 If Dist(sp) < Dist(result) Then Result = Sp Next Ms = 0 Select Case Result Case 1 Ms = 600 Call Turnleft Case 2 Ms = 500 Call Turnleft Case 3 Ms = 400 Call Turnleft Case 4 Ms = 200 Call Turnleft Case 5 Ms = 100 Call Turnleft Case 6 Ms = 50 Call Turnleft Case 7 Ms = 50 Call Turnright Case 8 Ms = 100 Call Turnright Case 9 Ms = 200 Call Turnright Case 10 Ms = 400 Call Turnright Case 11 Ms = 500 Call Turnright Case 12 Ms = 600 Call Turnright Case Else Sound Portd.7 , 1000 , 200 End Select End Sub Sub Turnleft() Text2 = "Linksdrehung" Call Lcdout() 'Print "drehen um " ; Ms ; " nach links" 'servo auf mitte stellen Servopos = Center Servo(1) = Servopos 'Linker Motor ein Portc.6 = 1 'bestimmt Richtung Portc.7 = 0 'bestimmt Richtung Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 0 'bestimmt Richtung rechter Motor Portb.1 = 1 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN 'beschleunigen der motoren V = 0 Do Pwm1a = V Pwm1b = V Waitms 20 V = V + 200 Loop Until V > 1000 Waitms Ms Call Bremsen Ms = 0 End Sub Sub Turnright() Text2 = "Rechtsdrehung" Call Lcdout() 'servo auf mitte stellen Servopos = Center Servo(1) = Servopos 'Linker Motor ein Portc.6 = 0 'bestimmt Richtung Portc.7 = 1 'bestimmt Richtung Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 1 'bestimmt Richtung rechter Motor Portb.1 = 0 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN 'beschleunigen der motoren V = 0 Do Pwm1a = V Pwm1b = V Waitms 20 V = V + 200 Loop Until V > 1000 Waitms Ms Call Bremsen Ms = 0 End Sub Sub Turn180() 'servo auf mitte stellen Text2 = "180 Grad Drehung" Call Lcdout() 'Linker Motor ein Portc.6 = 0 'bestimmt Richtung Portc.7 = 1 'bestimmt Richtung Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 1 'bestimmt Richtung rechter Motor Portb.1 = 0 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN 'beschleunigen der motoren If V = 0 Then Do Pwm1a = V Pwm1b = V Waitms 20 V = V + 100 Loop Until V > 1000 End If Waitms 1250 'warten 'ab Call Bremsen End Sub Sub Stopnow() Text2 = "Vollbremsung" Call Lcdout() V = 0 Pwm1a = V 'Linker Motor aus Pwm1b = V 'rechter Motor aus End Sub Sub Anfahren() Text2 = "Anfahren" Call Lcdout() 'Linker Motor ein Portc.6 = 0 'bestimmt Richtung Portc.7 = 1 'bestimmt Richtung Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 0 'bestimmt Richtung rechter Motor Portb.1 = 1 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN 'Beschleunigen V = 0 Do Pwm1a = V Pwm1b = V Waitms 10 V = V + 100 Loop Until V > 1000 End Sub Sub Bremsen() Text2 = "Bremsen" Call Lcdout() Do Pwm1a = V Pwm1b = V Waitms 20 V = V - 200 Loop Until V < 1 Pwm1a = 0 'Linker Motor aus Pwm1b = 0 End Sub Sub Rewind() Text2 = "Rewind..." Call Lcdout() 'Linker Motor ein Portc.6 = 1 'bestimmt Richtung Portc.7 = 0 'bestimmt Richtung Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 1 'bestimmt Richtung rechter Motor Portb.1 = 0 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN 'Beschleunigen V = 0 Do Pwm1a = V Pwm1b = V Waitms 10 V = V + 100 Loop Until V > 1000 Waitms 100 End Sub Sub Lcdout() Cls Locate 1 , 1 Lcd Name Locate 2 , 1 Lcd Text2 Call Temp Locate 3 , 1 Lcd "Temp= " ; Fusing(value , "#.#") ; " Grad " ; Dl Call Batteriespannung Locate 4 , 1 Lcd "Ubatt= " ; Fusing(volt , "#.##") ; " V" Incr Dl End Sub Sub Batteriespannung() Local W As Word Local Z As String * 10 Start Adc W = Getadc(6) Volt = W * Ref Volt = Volt * 5.2941 End Sub Sub Temp() I2cstart I2cwbyte Device I2cwbyte &HEE I2cstop I2cstart I2cwbyte Device I2cwbyte &HAA I2cstop I2cstart I2cwbyte Deviceread I2crbyte Lowtemp , Ack I2crbyte Hightemp , Nack I2cstop Value = Lowtemp * 10 If Hightemp = 128 Then Value = Value + 5 End If Value = Value / 10 End Sub Timer_irq: Timer1 = Timervorgabe Call Lcdout Return
Zitieren
Lesezeichen