AEP Gruppe B2

Aus HSHL Mechatronik
Zur Navigation springen Zur Suche springen
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