'############################################################################### '####################### Mobile Autonomous Robotic System ###################### '############################################################################### '## Autor: Gergely Mentsik ## '## LM: 01-07-2010 ## '## Web: www.techgeek.at ## '############################################################################### '## Software zur Steuerung des Hauptprozessors von MARS. Sowohl die künstliche## '## Intelligenz als auch die Fernsteuerung vom PC ist möglich. ## '############################################################################### '## Aufgaben: ## '## Funkdaten von PC annehmen und auswerten ## '## Daten zum PC senden ## '## Daten unter Umständen auf andere Prozessoren verteilen ## '## Kamerasteuerung durch Servos + Licht ## '## I2C Bus handling ## '## Erkennen von Hindernissen mittels US und IR Sensoren ## '## Motoransteuerung ## '## 2 US Sensoren SER02 ## '## 6 IR Sensoren GPD ## '## Kompasssensor auslesen ## '## Telemetrie senden ## '## Beschleunigungsmessung ## '## Schwenken des Kamerakopfes ## '############################################################################### '## Externe Komponenten ## '## RECHTER MOTOR: ## '## EN: PWM1B ## '## VOR: PORTC.2 ## '## ## '## LINKER MOTOR: ## '## EN: PWM1A ## '## VOR: PORTC.1 ## '## ## '## ## '## ## '## IR Vorne = Channel 3 ## '## IR Rechts = Channel 1 ## '## IR LINKS = Channel 3 ## '## ## '## US LINKS = &HE0 ## '## US RECHTS = &HE2 ## '## RADAR: Mitte: 211 LINKS:Radar = 186 Rechts: 235 ## '############################################################################### '=============================================================================== '=========================Beginn der Initialisierung============================ '=============================================================================== '\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\ '============================== Deklaration ==================================== '$sim $prog , 255 , &B11011001 , 'Quarz an / Teiler aus / Jtag aus $regfile = "m2560def.dat" 'Definitionsfile für M2560 Prozessor $hwstack = 82 '80 Hardwarestack $framesize = 68 '64 Framesize $swstack = 68 '44 Softwarestack $baud = 19200 'Baudrate für UART $crystal = 16000000 'Quarzfrequenz '================================ Port Konfiguration =========================== Config Timer0 = Pwm , Compare A Pwm = Clear Up , Prescale = 256 'CAM PAN PWM Timer Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 1 'Motor PWM Config Timer2 = Pwm , Compare B Pwm = Clear Up , Prescale = 256 ' CAM TILT PWM Config Timer4 = Pwm , Pwm = 8 , Compare A Pwm = Clear Up , Compare B Pwm = Clear Down , Compare C Pwm = Clear Up , Prescale = 256 'RADAR SERVO PWM Config Timer3 = Timer , Prescale = 64 'INTERRUPT On Timer3 Timer_irq 'WENN Timer dann spring zur Routine Timer_irq Enable Timer3 'Timer 3 = AN Config Adc = Single , Prescaler = Auto 'Config der Analog-Digital Converter 'PIN-Configs '=========== Config Pind.5 = Output Config Portc = Output Config Porta.0 = Output Config Porta.1 = Output Config Portb.0 = Output Config Porth.3 = Output Config Porth.4 = Output Config Porth.5 = Output Config Porte.4 = Input Config Porte.6 = Input Config Scl = Portd.0 'Ports fuer IIC-Bus Config Sda = Portd.1 'UART Config '============= Config Com2 = 19200 , Synchrone = 0 , Parity = None , Stopbits = 1 , Databits = 8 , Clockpol = 0 'Bluetooth Config Com3 = 4800 , Synchrone = 0 , Parity = None , Stopbits = 1 , Databits = 8 , Clockpol = 0 'GPS Config Com4 = 19200 , Synchrone = 0 , Parity = None , Stopbits = 1 , Databits = 8 , Clockpol = 0 'USB Config Serialout2 = Buffered , Size = 100 Config Serialin2 = Buffered , Size = 100 Config Serialout3 = Buffered , Size = 100 Config Serialin3 = Buffered , Size = 100 Config Serialin1 = Buffered , Size = 100 Config Serialout1 = Buffered , Size = 100 'Ports setzen '============ Porte.4 = 1 Porte.6 = 1 '=============================== INTERRUPT ROUTINEN ============================ '"Bumper" '========== Config Int4 = Rising Config Int6 = Rising Enable Int4 Enable Int6 On Int4 Bump_l Nosave On Int6 Bump_r Nosave 'Decoder für Drehzahlmessung '============================= Pcmsk1 = &B10000000 'zum messen der Umdrehungen LINKS, PCINT15 auswählen On PCI1 ISR_DECODERL 'PCINT15 springt zu ISR_DECODERL-Routine Enable PCI1 'PCINT15 schließlich aktivieren Config Int7 = Change 'INT7 soll fallende Flanken detektieren On Int7 ISR_DECODERR 'INT7 springt zu ISR_DECODERR-Routine Enable Int7 ' INT7 schließlich aktivieren Enable Interrupts 'alle Interrupts einschalten '=============================== Alias Sektion ================================= Kette_l_bwd Alias Portc.2 'für Motortreiber, Drehrichtung Kette_l_fwd Alias Portc.3 'für Motortreiber, Drehrichtung Kette_r_fwd Alias Portc.1 'für Motortreiber, Drehrichtung Kette_r_bwd Alias Portc.0 'für Motortreiber, Drehrichtung Kette_r_speed Alias Pwm1b 'Pulsweitenmodulation für Motoren, Rechts (Drehzahl) Kette_l_speed Alias Pwm1a 'Pulsweitenmodulation für Motoren, Links (Drehzahl) RADAR Alias PWM4C 'Pulsweitenmodulation für den IR-Radar vorne, für Servo Led_go Alias Portd.5 'Status LED am Controller Led_stat Alias Portb.0 'Status LED am Mainboard Porte.4 Alias Bumper_r 'Rechter Bumper Porte.6 Alias Bumper_l 'Linker Bumper CamTilt Alias PWM2B 'Kamerkopf, neigen CamPan Alias PWM0A 'Kamerakopf, schwenken '=============================== Variablen und Konstanten ====================== Const Srf02_slaveid_links = &HE0 'I2C Adresse des linken US Sensors Const Srf02_slaveid_rechts = &HE2 'I2C Adresse des rechten US Sensors Const Mega32_slaveid = &HE20 'I2CAdressefür Coprozessor 1 Const Mars_firmware = "0.4.1" 'Programmversion Const Mars_filename = "MARS_Mainpro.bas" 'Dateiname Const Mars_filedate = "01.07.2010" 'Datum der letzten Änderung Const Ir_s_threshold = 220 'Grenzwert für IR-Sensoren Const DtoP = 3.454619665 Const BAL = 100 Const BAR = 100 'PID Dim l_Ist As integer Dim r_Ist As Integer Dim l_Soll As Integer Dim r_Soll As Integer Dim Kp As Long Dim Ki as Long Dim Ta AS Long Dim Pidr As Integer Dim Pidl As Integer Dim Error As Long Dim Error_suml As Integer Dim Error_sumr As Integer Dim PID_Buffer As Integer Dim start_flag As Byte Dim T_ist As Integer Dim T_alt As Integer Dim T1 As Integer Dim Speed_l_PID as Byte Dim Speed_r_PID as Byte 'Puffer/Zähler Dim U As Byte 'benötigt für Zählschleifen, Puffervariable Dim i as Byte Dim Buffer1 As Single 'Puffer-Variable 1, common-use Dim Ir_buffer1 As Integer 'Puffer-Variable 2, fü gebrauch mit IR-Sensoren Dim Ir_buffer2 As Integer 'Puffer-Variable 3, fü gebrauch mit IR-Sensoren Dim SENS_Buffer As Word 'fahrt Dim Kette_Rechts As Integer 'benötigt für Fahrtprozess Dim Kette_Links As Integer 'benötigt für Fahrtprozess Dim inGPS as String * 100 'Kompass Dim Himmelsrichtung As Word 'beinhaltet Kompasswerte Dim Grad As Integer 'errechneter Grad-Wert aus Kompass 'Sensoren Dim Sens_srf02_l As Integer 'beinhaltet Werte des linken Ultraschallsensors Dim Sens_srf02_r As Integer 'beinhaltet Werte des rechten Ultraschallsensors Dim IR_SL As Word 'IR Sensor vorne Dim IR_SR As Word 'IR Sensor links Dim IR_RA As Word 'IR Sensor rechts Dim IR_HL As Word 'IR Sensor vorne Dim IR_HR As Word 'IR Sensor links Dim IR_BO As Word 'IR Sensor rechts Dim US_L As Word 'US-Sensor Links Dim US_R As Word 'US-Sensor Rechts Dim ACCX As word Dim ACCY As Word Dim ACCZ As Word Dim CMPS As Word Dim IR_Seite_Links As Word 'IR Seite Links, gefilterter Wert Dim IR_seite_rechts as Word 'IR Seite Rechts, gefilterter Wert Dim IR_hinten_links As Word 'IR hinten Links, gefilterter Wert Dim IR_hinten_rechts As Word 'IR hinten Rechts, gefilterter Wert Dim IR_Boden As Word Dim IR_RADAR As Word 'IR Boden, gefiltert Dim SENS_CMP As Word Dim SENS_ACC_X As Word Dim SENS_ACC_Y As Word Dim SENS_ACC_Z As Word 'UART Dim inByte As Byte Dim ServoNr as Byte Dim ServoPos As Byte Dim outByte As Word Dim stopDelay As Long Dim aut as Bit Dim line_f as Bit Dim Adress As Byte 'Adresse des Befehls aus UART Dim Cmd As Byte 'Kommando des Befehls aus UART Dim Check As Byte 'Checksumme der Daten aus UART Dim Mycheck As Byte 'gebildete Checksumme aus Daten von UART Dim inDrive As Byte 'zum auswählen der Ketten Dim inSpeedL As Integer 'Geschwindigkeit der linken Kette per UART Dim inSpeedR As Integer 'Geschwindigkeit der rechte Kette per UART Dim outWord As Word Dim outInt As Integer Dim Line_l As Word 'Liniensensor Links Dim Line_m As Word 'Liniensensor Mitte Dim Line_r As Word 'Liniensensor Rechts Dim SENS_LINE_R AS Word Dim SENS_LINE_M As Word Dim SENS_LINE_ As Word Dim Rechte_entfernung As Word 'Beinhlatet Rechte Entferung aus US-Modul Dim Linke_entfernung As Word 'Beinhlatet Linke Entferung aus US-Modul Dim Kettenspeedlinks As Integer 'beinhlatet linke Kettengeschwindigkeit Dim Kettenspeedrechts As Integer 'beinhlatet rechte Kettengeschwindigkeit Dim cmsL As Integer Dim cmsR As Integer Dim Left_error As Integer 'offset zwischen SOLL und IST-Wert der Linken-Seite Dim Right_error As Integer 'offset zwischen SOLL und IST-Wert der Rechten-Seite Dim Left_Distance As Word 'beninhaltet Linke Odometrie Dim Right_Distance As Word 'beinhaltet Rechte Odometrie Dim SPoutL As Integer Dim SPoutR As Integer '====FÜRS AUSWEICHEN======= Dim BumpL As Bit Dim BumpR As Bit Dim BumpACCL As Integer Dim BumpACCR As Integer Dim IR_RADAR_L As Word Dim IR_RADAR_R As Word Dim sumL As Word Dim sumR As Word Dim changeDir as Byte Dim US_L_Entfernung As Word Dim US_R_Entfernung As Word '========================= FUNKTIONEN UND SUB's ================================ Declare Function Srf02_firmware(byval Slaveid As Byte) As Byte 'US Sensor Subrutine 1 (Firmware Ermittlung) Declare Function Srf02_entfernung(byval Slaveid As Byte) As Integer 'US Sensor Subrutine 2 (Entferung Auslesen) Declare Function getSens(byval Sens As String) As Word 'Sensorenhandling Declare Function Cmps_firmware() As Byte 'Kompass Firmware ermitteln Declare Function Heading()as Word 'Richtung ermitteln Declare Function Getline() As Bit 'Linie ermitteln Declare Function GetSpeed(byVal Side as String) As Integer 'ermitteln der Kettengeschwindigkeit mittel Sigmoid-Funktion Declare Function Radar_Dist() As Word 'RADAR-Handling Declare Function sigm(byval x As Word) As Integer 'Unterprogramm für die Bluetoothkommunikation Declare Sub Drive(byval Speed_l As Integer , Byval Speed_r As Integer) 'Unterprogramm fürs fahren Declare Sub UART() Declare Sub GPS() Declare Sub Ausweichman() '=============================== INIT ========================================== Init: '#1 Initialisierung einiger Varibeln '================================== Left_Distance = 0 Right_Distance = 0 l_ist = 0 r_ist = 0 l_soll = 0 r_soll = 0 Kette_Links = 0 'Linke Kettengeschwindigkeit holen Kette_Rechts = 0 start_flag = 0 Kp = 1.4 Ki = 0 aut = 0 Led_go = 0 Led_stat = 1 BumpACCL = 0 BumpACCR = 0 changeDir = 0 '#2 öffnen der COM-Ports '========================= Open "COM2:" For Binary As #2 Open "COM3:" For Binary As #3 Open "COM4:" For Binary As #4 'COM-Port öffnen '#3 Analog-Digital Converter starten '=================================== Start Adc '#4 I2C-Schnittstelle initialisieren '=================================== 'Analog-Digitalwandler starten I2cinit '#5 Servos setzen '================= 'TWI-Bus aktivieren, initialisieren RADAR = 211 CamTilt = 215 CamPan = 215 '#6 Kamera = AUS '================ I2cstart I2cwbyte &H20 I2cwbyte 23 I2cstop '#7 Initialiserung fertig Print #2 , "=============" Print #2 , Mars_firmware 'Ausgabe der Firmware Print #2 , Mars_filename 'Ausgabe des Programmnamens Print #2 , Mars_filedate 'Ausgabe der letzten Änderung Print #2 , "=============" Print #2 , "Ready" RADAR = 186 wait 2 RADAR = 232 wait 2 RADAR = 211 '=============================================================================== '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '=================================MAIN LOOP===================================== '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '=============================================================================== Do Call UART() If aut = 1 Then IR_RADAR = Radar_Dist() If IR_RADAR > 280 Then Call Ausweichman() Kette_Links = GetSpeed( "Left") 'Linke Kettengeschwindigkeit holen Kette_Rechts = GetSpeed( "Right") 'Rechte Kettengeschwindigkeit holen 'Radarwert abfragen, Servo Positionieren IR_Boden = getSens( "IR_BO") BumpACCL = BAL * BumpL BumpACCR = BAR * BumpR Kette_Links = Kette_Links + BumpACCL Kette_Rechts = Kette_Rechts + BumpACCR Kette_Links = Kette_Links Kette_Rechts = Kette_Rechts If BumpL = 1 And BumpR = 1 Then Call Ausweichman Call Drive(Kette_Links , Kette_Rechts) BumpL = 0 BumpR = 0 End If BumpL = 0 BumpR = 0 Loop '=============================================================================== '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '==============================ENDE MAIN LOOP=================================== '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '=============================================================================== '================================INTERRUPT HANDLING ============================ Bump_r: 'Kollisionssensor-rechts BumpR = 1 Return Bump_l: 'Kollisionssensor-links BumpL = 1 Return Isr_decoderl: 'Odometriezähler Rechts Incr Left_distance 'If Kette_rechts >= 0 Then Left_distance = Left_distance + 1 'If Kette_rechts <= 0 Then Left_distance = Left_distance - 1 Return Isr_decoderr: Incr Right_distance 'Odometriezähler Links 'If Kette_links >= 0 Then Right_distance = Right_distance + 1 'If Kette_links <= 0 Then Right_distance = Right_distance - 1 Return TIMER_IRQ: Timer3 = 3036 'incr T1 toggle Led_stat l_soll = Kette_links r_soll = Kette_rechts 'l_ist = sigm2(Left_Distance) 'r_ist = sigm2(Right_Distance) Left_Distance = Left_Distance Right_Distance = Right_Distance l_ist = Left_Distance * 15.02182739 r_ist = Right_Distance * 15.08135837 'l_ist = Left_Distance * 3.518304078 'r_ist = Right_Distance * 3.622007069 Left_distance = 0 Right_distance = 0 Return '############################################################################### '################################ Sub Prozesse ################################# '############################################################################### '\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\ '============================== IR RADAR ======================================== Sub Ausweichman() 'Sub-Name = Ausweichman Call Drive(0 , 0) 'Zunächst stehen bleiben Radar = 186 'Radar nach LINKS Drehen wait 1 'eine Sekunde warten, damit die Messung nicht vor dem erreichen der Servoposition geschieht IR_RADAR_L = GetSens( "IR_RA") 'IR Sensor auslesen Radar = 233 'Radar nach RECHTS drehen wait 1 'eine Sekunde warten, damit die Messung nicht vor dem erreichen der Servoposition geschieht 'nach Links schauen IR_RADAR_R = GetSens( "IR_RA") ' IR Sensor auslesen IR_Seite_Links = GetSens( "IR_SL") 'IR Sensor Seite Links auslesen IR_Seite_rechts = GetSens( "IR_SR") 'IR Sensor Seite Rechts If IR_RADAR_L < 120 And IR_Seite_Links < 150 Then 'Wenn LINKS Vorne (RADAR) UND LINSK Seitlich der Abstand größer als ca 20cm DANN.... Call Drive( -185 , 185) 'Drehe dich nach LINKS Elseif IR_RADAR_R < 120 AND IR_Seite_rechts < 120 Then 'Anderfalls WENN RECHTS Vorne (RADAR) und RECHTS Seitlich Abstand größer als ca20cm DANN Call Drive(185 , -185) 'Drehe dich nach RECHTS Else 'Anderfalls IR_hinten_rechts = GetSens( "IR_HR") 'IR Sensor HINTEN RECHTS auslesen IR_hinten_links = GetSens( "IR_HL") 'IR SENSOR HINTEN LINKS auslesen If IR_Hinten_links < 210 AND IR_Hinten_rechts < 210 Then 'WENN HINTEN ALLES FREI DANN Call Drive( -200 , -200) 'FAHR Rückwärts End If End if waitms 1500 'Die ausgewählte Aktion wird 1,5 Sekunden lang durchgeführt End Sub '====================================GPS======================================== Sub GPS() if ischarwaiting(#3) = 1 Then inGPS = inkey(#3) Print #2 , inGPS End If End Sub '============================ KOMMANDO AUSWERTUNG ============================== Sub UART() if ischarwaiting(#2) = 1 Then inByte = inkey(#2) 'Print #2 , "Recieved: "; 'Print #2 , inByte If ServoNr > 0 Then Select Case ServoNr Case 1: RADAR = inByte Case 2: CamTilt = inByte Case 3 : CamPan = inByte End Select ServoNr = 0 Exit Sub End If If inDrive > 0 Then Select Case inDrive Case 1: inSpeedL = inByte Case 2: inSpeedR = inByte Case 3: inSpeedL = inByte * -1 Case 4: inSpeedR = inByte * -1 End Select inDrive = 0 Exit Sub End If If inByte = 45 Then Print #2 , "Bluetooth connected" Exit Sub End If If inByte = 31 Then I2cstart 'TWI-Bus starten I2cwbyte &H20 'Slave-Adresse für Mega32-Sub-Prozessor auf Bus schreiben I2cwbyte 23 'Byte 23 = Kamera: AN I2cstop Exit Sub 'TWI-Bus wieder freigeben End If If inByte = 32 Then I2cstart 'TWI-Bus starten I2cwbyte &H20 'Slave-Adresse für Mega32-Sub-Prozessor auf Bus schreiben I2cwbyte 24 'Byte 24= Kamera: AUS I2cstop Exit Sub 'TWI-Bus wieder freigeben End If If inByte = 33 Then I2cstart 'TWI-Bus starten I2cwbyte &H20 'Slave-Adresse für Mega32-Sub-Prozessor auf Bus schreiben I2cwbyte 25 'Byte 24= Kamera: AUS I2cstop Exit Sub 'TWI-Bus wieder freigeben End If If inByte = 34 Then I2cstart 'TWI-Bus starten I2cwbyte &H20 'Slave-Adresse für Mega32-Sub-Prozessor auf Bus schreiben I2cwbyte 26 'Byte 24= Kamera: AUS I2cstop Exit Sub 'TWI-Bus wieder freigeben End If If inByte = 35 Then I2cstart 'TWI-Bus starten I2cwbyte &H20 'Slave-Adresse für Mega32-Sub-Prozessor auf Bus schreiben I2cwbyte 27 'Byte 24= Kamera: AUS I2cstop Exit Sub 'TWI-Bus wieder freigeben End If If inByte = 36 Then I2cstart 'TWI-Bus starten I2cwbyte &H20 'Slave-Adresse für Mega32-Sub-Prozessor auf Bus schreiben I2cwbyte 28 'Byte 24= Kamera: AUS I2cstop Exit Sub 'TWI-Bus wieder freigeben End If If inByte = 41 Then ServoNr = 1 Exit Sub 'Servo auswählen nr1= IR_RADAR End If If inByte = 42 Then ServoNr = 2 Exit Sub 'Servo auswählen nr2= Kamera-Tilt End If If inByte = 43 Then ServoNr = 3 Exit Sub 'Servo auswählen nr3= Kamera-Pan End If If inByte = 101 Then inDrive = 1 Exit Sub 'inDrive=1 Kette Links Vorwärts auswählen End If If inByte = 102 Then inDrive = 2 Exit Sub 'inDrive=2 Kette Rechts Vorwärts auswählen End If If inByte = 103 Then inDrive = 3 Exit Sub 'inDrive=3 Kette Links Rückwärts auswählen End If If inByte = 104 Then inDrive = 4 Exit Sub 'inDrive=4 Kette Rechts Rückwärts auswählen End If If inByte = 105 Then Call Drive(inSpeedL , inSpeedR) Exit Sub 'inDrive=4 Kette Rechts Rückwärts auswählen End If If inByte = 106 Then Call Drive(0 , 0) 'Not-stop wait 20 Exit Sub End If If inByte = 111 Then 'US-Sensor links outByte = getSens( "US_L") 'Wert auslesen Print #2 , "#,111," ; outByte ; ",*" 'verpacken und ausgeben inByte = 0 'Variable löschen outByte = 0 'Variable löschen Exit Sub 'Sub verlassen End If If inByte = 112 Then outByte = getSens( "US_R") 'US-Sensor rechts Print #2 , "#,112," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 113 Then outByte = getSens( "IR_SL") 'IR Sensor seite links Print #2 , "#,113," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 114 Then 'IR Sensor Seite Rechts outByte = getSens( "IR_SR") Print #2 , "#,114," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 115 Then 'IR Sensor hinten Rechts outByte = getSens( "IR_HL") Print #2 , "#,115," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 116 Then 'IR Sensor Hinten Rechts outByte = getSens( "IR_HR") Print #2 , "#,116," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 117 Then 'IR Sensor Radar outByte = getSens( "IR_RA") Print #2 , "#,117," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 118 Then outByte = getSens( "IR_BO") 'IR Sensor Boden Print #2 , "#,118," ; outByte ; ",*" inByte = 0 outByte = 0 Exit Sub End If If inByte = 119 Then 'linke ermittelte Drehzahl 'outByte = l_ist 'Not-stop SPoutL = GetSpeed( "Left") Print #2 , "#,119," ; SPoutL ; ",*" 'Linke Kettengeschwindigkeit holen inByte = 0 outByte = 0 Exit Sub End If If inByte = 120 Then 'rechte ermittelte Drehzahl 'outByte = r_ist SPoutR = GetSpeed( "Right") Print #2 , "#,120," ; SPoutR ; ",*" inByte = 0 outByte = 0 Exit Sub End If '=================================================================== If inByte = 121 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.ACC.X") Print #2 , "#,121," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 122 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.ACC.Y") Print #2 , "#,122," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 123 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.ACC.Z") Print #2 , "#,123," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 124 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.CMP") Print #2 , "#,124," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 125 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.LINE.L") Print #2 , "#,125," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 126 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.LINE.M") Print #2 , "#,126," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 127 Then 'rechte ermittelte Drehzahl outWord = getSens( "SENS.LINE.R") Print #2 , "#,127," ; outWord ; ",*" inByte = 0 outWord = 0 Exit Sub End If If inByte = 150 Then 'STOP Call Drive(0 , 0) End If If inByte = 200 Then 'autonomes fahren AN Print #2 , "#,200,AUT=AN,*" aut = 1 inByte = 0 outByte = 0 Exit Sub End If If inByte = 201 Then 'autnomes fahren AUS Print #2 , "#,201,AUT=AUS,*" aut = 0 Call Drive(0 , 0) RADAR = 211 inByte = 0 outByte = 0 Exit Sub End If outByte = 0 Else Incr StopDelay If stopDelay >= 400000 Then Call Drive(0 , 0) 'regelmäßig den Roboter stoppen StopDelay = 0 'falls Verbindung unterbrochen wird End if End If End Sub '=========================== PROZESS ZUM FAHREN ================================ Sub Drive(speed_l As Integer , Speed_r As Integer) If Speed_l => 255 Then Speed_l = 255 If Speed_r => 255 Then Speed_r = 255 If Speed_l =< -255 Then Speed_l = -255 If Speed_r =< -255 Then Speed_r = -255 If Speed_l =< 255 Or Speed_r =< 255 Then If Speed_l >= -255 Or Speed_r >= -255 Then Kette_l_fwd = 1 Kette_l_bwd = 0 Kette_r_fwd = 1 Kette_r_bwd = 0 If Speed_l < 0 Then Speed_l = Speed_l * -1 Kette_l_fwd = 0 Kette_l_bwd = 1 End If If Speed_r < 0 Then Speed_r = Speed_r * -1 Kette_r_fwd = 0 Kette_r_bwd = 1 End If Kette_l_speed = Speed_l Kette_r_speed = Speed_r End If End If If Speed_l = 0 And Speed_r = 0 Then Kette_l_speed = 0 Kette_r_speed = 0 Kette_l_fwd = 1 Kette_l_bwd = 1 Kette_r_fwd = 1 Kette_r_bwd = 1 End If End Sub '############################################################################### '########################### FUNKTIONEN ###################################### '############################################################################### '============================== IR RADAR ======================================== Function Radar_Dist() As Word 'Funktion RADAR_Dist liefert eine Word-Varible zurück Buffer1 = Kette_Links - Kette_Rechts 'Roboter Fahrtrichtung ermitteln Buffer1 = Buffer1 / 2 'Wir benötigen den Wertebereich 0-47 Buffer1 = 211 + Buffer1 'Der errechnete Wert wir zur Mitte addiert: 211+20 z.b. RADAR = Buffer1 'Servo setzen 'Wenn sich der Roboter dreht Servo ganz ausschwenken 'Wenn Kette Links = Zurück und Rechts = Vor DANN = Drehung nach LINKS If Kette_Links < -1 AND KETTE_RECHTS > 1 Then RADAR = 186 'RADAR ganz nach Links drehen End If If KETTE_RECHTS < -1 AND Kette_Links > 1 Then 'Wemnn Kette Rechts Zurück und KEtte LINKS VOR dann Drehung nach Rechts also... RADAR = 233 'Radar ganz nach Rechts drehen End If waitms 250 ' kurz warten, damit der Servo mitkommt und erst beim errecihend er servopostion gemessen wird Radar_Dist = getSens( "IR_RA") 'nun IR-Abstandsmessung durchführen End Function ' Ende der Funkion '======================= BERECHNUNG DER KETTENDREHZAHL ========================= Function GetSpeed(byVal Side as String) As Integer If Side = "Right" Then 'ermitteln der verlangten Seite Rechte_entfernung = getSens( "US_R") Linke_entfernung = getSens( "US_L") Kettenspeedrechts = sigm(Linke_entfernung) IR_Seite_links = getSens( "IR_SL") 'seitlichen Rechten IR-Sensor auslesen If IR_Seite_links <= 240 Then 'Wenn Rechter IR-Sensor > 10 cm Left_error = 0 'Linker-offset = 0 Else 'Ansonsten... Left_error = IR_Seite_links - Ir_s_threshold 'Linker-offset = IST Wert - Soll Wert Left_error = Left_error / 10 If Kettenspeedrechts < -1 Then Left_error = Left_error * -1 ' Linker-offset = Linker-offset / 10 (damit in Bereich der Motor-Pulsweitenmodulation End If ' Ende Wenn 'Linken US Abfragen If Rechte_entfernung = Linke_entfernung And Rechte_entfernung < 70 THen IR_Seite_links = getSens( "IR_SL") IR_Seite_Rechts = getSens( "IR_SR") Left_error = IR_seite_rechts - IR_Seite_links left_error = Left_error / 10 GetSpeed = Kettenspeedrechts - Left_error Exit Sub End if GetSpeed = Kettenspeedrechts - Left_error 'Nun ziehen wir noch den Offset ab damit man nciht an der Wand schleift Exit Sub End If If Side = "Left" Then Rechte_entfernung = getSens( "US_R") 'Kette = US Entfernung in cm * 2.55 Linke_entfernung = getSens( "US_L") Kettenspeedlinks = sigm(Rechte_entfernung) 'Kette = US Entfernung in cm * 2.55 IR_Seite_Rechts = getSens( "IR_SR") If IR_Seite_Rechts <= 240 Then Right_error = 0 Else Right_error = IR_Seite_Rechts - Ir_s_threshold Right_error = Right_error / 10 If Kettenspeedlinks < -1 Then Right_error = Right_error * -1 End If If Rechte_entfernung = Linke_entfernung And RECHTE_ENTFERNUNG < 70 THen IR_Seite_links = getSens( "IR_SL") IR_Seite_Rechts = getSens( "IR_SR") Right_error = IR_Seite_links - IR_seite_rechts Right_error = Left_error / 10 GetSpeed = Kettenspeedlinks - Right_error Exit Sub End if GetSpeed = Kettenspeedlinks - Right_error End If End Function '========= Sigmoidfunktion zum Koppeln der Sensoren mit den Motoren ============ 'geniale Sigmoid-Funktion meinerseits.... Function sigm(byval x As word) As Integer buffer1 = x - 70 'Funktion wird nach rechts verschoben buffer1 = buffer1 * -1 '-1, da sigmoidfunktion gedreht wird buffer1 = buffer1 / 8 'möglichst steil damit schneller reagiert wird buffer1 = 2.718281828 ^ buffer1 ' eulersche Zahl hoch unserem Wert buffer1 = 1 + buffer1 'standart Sigmoidfunktion buffer1 = 1 / buffer1 ' 1/, standart bei Sigmoid buffer1 = buffer1 - 0.5 'Funktion nach oben verschieben, damit die werteberecihe passen buffer1 = buffer1 * 440 'Alle mal 510 damit wird die Werte direkt anwenden können If buffer1 => -185 and buffer1 =< 0 Then buffer1 = buffer1 ^ 4 buffer1 = buffer1 / 80000000 buffer1 = buffer1 + 180 buffer1 = buffer1 * -1 End If If buffer1 =< 185 and buffer1 >= 0 Then buffer1 = buffer1 ^ 4 buffer1 = buffer1 / 80000000 buffer1 = buffer1 + 180 End if sigm = int(buffer1) 'Wir brauchen nur die ganzzahligen Anteile End Function '====================== Funktion zum ermitteln einer Linie ===================== Function Getline() As Bit Line_r = Getadc(6) Line_m = Getadc(5) Line_l = Getadc(4) Print #2 , "Rechts: "; Print #2 , Line_r Print #2 , "Mitte: "; Print #2 , Line_m Print #2 , "Links: "; Print #2 , Line_l End Function '============================ SENSORENENABFRAGE ================================ Function getSens(byval Sens As String) As Word If Sens = "US_L" Then For I = 0 To 2 Sens_srf02_l = Srf02_entfernung(srf02_slaveid_links) 'US Links US_L = US_L + Sens_srf02_l Next I getSens = US_L / 3 US_L = 0 Exit Sub End If If Sens = "US_R" Then For I = 0 To 2 Sens_srf02_r = Srf02_entfernung(srf02_slaveid_rechts) 'US Rechts US_R = US_R + Sens_srf02_r Next I getSens = US_R / 3 US_R = 0 Exit Sub End If If Sens = "IR_BO" Then For I = 0 To 5 Ir_buffer1 = Getadc(7 , 64) Ir_bo = Ir_bo + Ir_buffer1 Next I getSens = Ir_bo / 6 Ir_bo = 0 Exit Sub End If If Sens = "IR_SL" Then For I = 0 To 5 Ir_buffer1 = Getadc(2) Ir_sl = Ir_sl + Ir_buffer1 Next I getSens = Ir_sl / 6 Ir_sl = 0 Exit Sub End If If Sens = "IR_SR" Then For I = 0 To 5 Ir_buffer1 = Getadc(3) Ir_sr = Ir_sr + Ir_buffer1 Next I getSens = Ir_sr / 6 Ir_sr = 0 Exit Sub End If If Sens = "IR_HR" Then For I = 0 To 5 Ir_buffer1 = Getadc(2 , 64) Ir_hr = Ir_hr + Ir_buffer1 Next I getSens = Ir_hr / 6 ir_hr = 0 Exit Sub End If If Sens = "IR_HL" Then For I = 0 To 5 Ir_buffer1 = Getadc(6 , 64) Ir_hl = Ir_hl + Ir_buffer1 Next I getSens = Ir_hl / 6 ir_hl = 0 Exit Sub End If If Sens = "IR_RA" Then For I = 0 To 5 Ir_buffer1 = Getadc(1) Ir_ra = Ir_ra + Ir_buffer1 Next I getSens = Ir_ra / 6 ir_ra = 0 Exit Sub End If If Sens = "SENS.CMP" Then For I = 0 To 2 SENS_BUFFER = Heading() CMPS = CMPS + SENS_BUFFER Next I getSens = CMPS CMPS = 0 End If If Sens = "SENS.ACC.X" Then For I = 0 To 5 SENS_BUFFER = getadc(4) ACCX = ACCX + SENS_BUFFER Next I getSens = ACCX / 6 ACCX = 0 Exit Sub End If If Sens = "SENS.ACC.Y" Then For I = 0 To 5 SENS_BUFFER = getadc(4) ACCY = ACCY + SENS_BUFFER Next I getSens = ACCY / 6 ACCY = 0 Exit Sub End If If Sens = "SENS.ACC.Z" Then For I = 0 To 5 SENS_BUFFER = getadc(4) ACCZ = ACCZ + SENS_BUFFER Next I getSens = ACCZ / 6 ACCZ = 0 Exit Sub End If If Sens = "SENS.LINE.L" Then For I = 0 To 5 SENS_BUFFER = getAdc(4) Line_L = Line_L + SENS_BUFFER Next I getSens = Line_L / 6 Line_L = 0 Exit Sub End If If Sens = "SENS.LINE.M" Then For I = 0 To 5 SENS_BUFFER = getAdc(5) Line_m = Line_m + SENS_BUFFER Next I getSens = Line_m / 6 Line_m = 0 Exit Sub End If If Sens = "SENS.LINE.R" Then For I = 0 To 5 SENS_BUFFER = getAdc(6) Line_r = Line_r + SENS_BUFFER Next I getSens = Line_r / 6 Line_r = 0 Exit Sub End If End Function '========== Funktion zum auslesen der Firmware des US-Sensors per TWI ========== Function Srf02_firmware(byval Slaveid As Byte) As Byte Local Firmware As Byte Local Slaveid_read As Byte Slaveid_read = Slaveid + 1 I2cstart I2cwbyte Slaveid I2cwbyte 0 'Leseregister festlegen I2cstop I2cstart I2cwbyte Slaveid_read I2crbyte Firmware , Nack I2cstop Srf02_firmware = Firmware End Function '========= Funktion zum auslesen der Entfernung des US-Sensors per TWI ========= Function Srf02_entfernung(byval Slaveid As Byte) As Integer Local Lob As Byte Local Hib As Byte Local Firmware As Byte Local Temp As Byte Local Slaveid_read As Byte Slaveid_read = Slaveid + 1 'Messvorgang in starten I2cstart I2cwbyte Slaveid I2cwbyte 0 I2cwbyte 81 'in Zentimetern messen I2cstop Warteaufmessung: Firmware = Srf02_firmware(slaveid) If Firmware = 255 Then Goto Warteaufmessung I2cstart I2cwbyte Slaveid I2cwbyte 2 'Leseregister festlegen I2cstop I2cstart I2cwbyte Slaveid_read I2crbyte Hib , Ack I2crbyte Lob , Nack I2cstop Srf02_entfernung = Makeint(lob , Hib) End Function '======= Funktion zum auslesen der Firmware des KOMPASS-Sensors per TWI ======== Function Cmps_firmware() As Byte Local Firmware As Byte Local Cmps_slaveid As Byte Local Cmps_slaveid_read As Byte Cmps_slaveid = &HC0 Cmps_slaveid_read = Cmps_slaveid + 1 I2cstart I2cwbyte Cmps_slaveid I2cwbyte 0 'Leseregister festlegen I2cstop I2cstart I2cwbyte Cmps_slaveid_read I2crbyte Firmware , Nack I2cstop Cmps_firmware = Firmware End Function '=================== BLICKRICHTUNG per TWI aus CMP-auslesen ==================== Function Heading() As Word Local Lob As Byte Local Hib As Byte Local Cmps_slaveid As Byte Local Cmps_slaveid_read As Byte Local Cmps_himmelsrichtung As Word Cmps_slaveid = &HC0 Cmps_slaveid_read = Cmps_slaveid + 1 'Register auswählen I2cstart I2cwbyte Cmps_slaveid I2cwbyte 2 I2cstop I2cstart I2cwbyte Cmps_slaveid_read I2crbyte Hib , Ack I2crbyte Lob , Nack I2cstop Cmps_himmelsrichtung = Makeint(lob , Hib) Heading = Cmps_himmelsrichtung / 10 Waitms 1 End Function '=============================================================================== '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ '#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=# '\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\ '{{{{{{{{{{{{{{{{{{{{{{{{{{{{{} ENDE M.A.R.S.{}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}} '\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\ '#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=#=# '===============================================================================