|
|
| 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 2014a 32-Bit und [[RWTH_Aachen_Toolbox_meets_NXT]].
| |
|
| |
| == 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);
| |