AEP 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
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 2014a 32-Bit und RWTH_Aachen_Toolbox_meets_NXT.
Quellcode
<syntaxhighlight lang="matlab" style="background-color: #EFF1C1; font-size:larger">
%% 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);
<\syntaxhighlight>
→ zurück zum Hauptartikel: Informatikpraktikum 2 - SoSe14
--~~~~ Tipps:
- Quellcode absetzen (s.o.)
- Inhalt ist übnersichtlich
- Teammitglieder auf USER-Seite verlinken