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