|
|
| (2 dazwischenliegende Versionen desselben Benutzers werden nicht angezeigt) |
| Zeile 1: |
Zeile 1: |
| [[Datei:AEP Gruppe B2 Bild 1.JPG|200px|thumb|right|Fahrzeug Gruppe B2]]Im SoSe14 wurde im Rahmen des Informatikpraktikums II die Aufgabe gestellt, ein autonom einparkendes Fahrzeug zu entwickeln. Als Grundlage diente ein LEGO Mindstorms NXT. Entwickelt wurde dieses Fahrzeug von Dominik Bräker, Nina Gorski und Sebastian Henneböhl.
| |
|
| |
|
| == Aufbau ==
| |
| [[Datei:AEP Gruppe B2 Bild 2.JPG|200px|thumb|right|Seitenansicht]]
| |
| == Bauplan ==
| |
| Hier ist der Bauplan für den LEGO Digital Designer zu finden. [[Datei:AEP Gruppe B2.lxf.zip]]
| |
| == Software ==
| |
| Programmiert wurde der NXT mithilfe von MATLAB und der zusätzlichen RWTH-Toolbox.
| |
| == Quellcode ==
| |
|
| |
| %% Alles reseten zum Start
| |
| COM_CloseNXT all
| |
| clear all
| |
| close all
| |
| %% USB Verbindung herstellen
| |
| handle = COM_OpenNXT();
| |
| COM_SetDefaultNXT(handle);
| |
| %% Ports
| |
| nGyroPort = SENSOR_2;
| |
| %% Init Gyro
| |
| OpenGyro(nGyroPort);
| |
| %% Parameter eingeben
| |
| power = 50;
| |
| durchmesser = 0.056; %im Meter
| |
| radumfang = durchmesser * pi;
| |
| fahrzeuglaenge = 0.32; %in m
| |
| luecke = fahrzeuglaenge * 1.3; %1,5 fache größe des fahrzeugs
| |
| lueckegrad = round(luecke/radumfang * 360);
| |
| strecke = 1; %in m
| |
| StreckeVorfahren = 0.1; % 10 cm vorfahren
| |
| grad = round(strecke/radumfang * 360); %2046 ; % 2046,28 Grad(errechnet durch den Radumfang) entspricht 1m
| |
| %% Motorfunktionen
| |
| motorA = NXTMotor(MOTOR_A, 'Power', power, 'TachoLimit', 0,'SmoothStart', true); %% Unendlich lange fahren
| |
| motorC = NXTMotor(MOTOR_C,'HoldBrake');
| |
| motorA.SendToNXT();
| |
| motorC.SendToNXT();
| |
| %% Set up ports
| |
| CloseSensor(SENSOR_4);
| |
| OpenUltrasonic(SENSOR_4);
| |
| %% Motor vorbereiten
| |
| bParklueckeGefunden = false;
| |
| XStart =0;
| |
| %% Parlücke suchen
| |
| while(bParklueckeGefunden == false)
| |
| %% Messwerte holen
| |
| distance = GetUltrasonic(SENSOR_4); % in cm
| |
| data = motorA.ReadFromNXT();
| |
| XAktuell = data.TachoCount*radumfang/360; % Aktuelle Distanz in m
| |
| if (distance < 15) % Parklücke besetzt
| |
| XStart = XAktuell;
| |
| else
| |
| % Parklücke frei
| |
| LueckenLaenge = XAktuell-XStart;
| |
| disp([num2str(LueckenLaenge),'m >',num2str(luecke),'m'])
| |
| if LueckenLaenge > luecke
| |
| % Luecke gefunden
| |
| motorA.Stop('off');
| |
| bParklueckeGefunden = true;
| |
| end
| |
| end
| |
| end
| |
| % 10 cm vorfahren
| |
| motorA = NXTMotor(MOTOR_A, 'Power', power, 'TachoLimit', round(StreckeVorfahren/radumfang * 360)); %% Unendlich lange fahren
| |
| motorA.SendToNXT();
| |
| motorA.WaitFor();
| |
| %% Einparken State Machine
| |
| % Gyro reset -> Winkel = 0
| |
| CalibrateGyro(nGyroPort, 'AUTO');
| |
| pause(1) % NICHT BEWEGEN
| |
| % Erst jetzt wieder Motor an!!!!
| |
| nState = 1;
| |
| bEinparkenErfolgreich = false;
| |
| Gierrate = 0;
| |
| GierWinkel =0;
| |
| tic
| |
| tAlt=0; % Init Stoppuhr
| |
| motorA = NXTMotor(MOTOR_A, 'Power', -power * 0.7, 'TachoLimit', 0,'SmoothStart', true); %% Unendlich lange fahren
| |
| motorA.SendToNXT();
| |
| motorC = NXTMotor(MOTOR_C, 'Power', -20, 'TachoLimit', 40); %% Unendlich lange lenken
| |
| motorC.SendToNXT();
| |
| while(bEinparkenErfolgreich==false)
| |
| %% Gyro auslesen
| |
| Gierrate = GetGyro(nGyroPort);
| |
| tNeu = toc;
| |
| dt = tNeu -tAlt; % Zeit seit letztem Durchgang
| |
| GierWinkel = GierWinkel + Gierrate*dt; % in deg
| |
| switch nState
| |
| case 1
| |
| disp(['1: ',num2str(GierWinkel),' < -40'])
| |
| if (GierWinkel< -36)
| |
|
| |
| motorC = NXTMotor(MOTOR_C, 'Power', 20, 'TachoLimit', 80); %% Unendlich lange lenken
| |
| motorC.SendToNXT();
| |
| nState = 2;
| |
| end;
| |
|
| |
|
| |
| case 2
| |
| disp(['2: ',num2str(GierWinkel),' >0'])
| |
| if (GierWinkel > - 18)
| |
|
| |
| motorA.Stop('off');
| |
| motorC.Stop('off');
| |
| motorC = NXTMotor(MOTOR_C, 'Power', -40);%, 'TachoLimit', 20); %% Unendlich lange lenken
| |
| motorC.SendToNXT();
| |
| %motorA.WaitFor();
| |
| motorA = NXTMotor(MOTOR_A, 'Power', 20, 'TachoLimit', 175); %% Vorwärts fahren
| |
| %TachoLimit = 1000;
| |
| motorA.SendToNXT();
| |
|
| |
| motorA.WaitFor();
| |
|
| |
| nState = 3;
| |
|
| |
| end;
| |
|
| |
| case 3
| |
|
| |
| data=motorA.ReadFromNXT();
| |
| disp(['3: ',num2str(data.TachoLimit),' > 300'])
| |
| if (data.TachoLimit >= 10000)
| |
|
| |
| % MOTORE STOPPEN
| |
| motorA.Stop('off');
| |
| motorC.Stop('off');
| |
| %nState = 4;
| |
|
| |
| end;
| |
| nState = 4;
| |
| case 4
| |
| motorA.Stop('off');
| |
| motorC.Stop('off');
| |
| NXT_PlayTone(440, 100); % Fanfare
| |
| bEinparkenErfolgreich = true;% Parkvorgang abgeschlossen
| |
| end
| |
| tAlt = tNeu; % Zeit merken;
| |
| end;
| |
| %% Alles aufräumen
| |
| CloseSensor(SENSOR_4);
| |
| CloseSensor(nGyroPort);
| |
| motorA.Stop('off');
| |
| motorC.Stop('off');%wegen 'Hold Brake'
| |
| COM_CloseNXT(handle);
| |