AEP Gruppe B2

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

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

---

→ zurück zum Hauptartikel: Informatikpraktikum 2 - SoSe14

--Ulrich Schneider (Diskussion) 08:31, 19. Jun. 2014 (CEST) Tipps:

• Quellcode absetzen (s.o.)
• Inhalt ist übnersichtlich
• Teammitglieder auf USER-Seite verlinken