Kategorie: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

 %% 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);