red_beast8

leggor.de -> nxt -> red_beast

<< >>

// software:
// ---------
// compiler:  nbc.exe               - version 1.0.1.29
// ide:       bricx commandcenter   - version 3.3
// firmware:  lego nxt              - version 1.03
// os:        windows xp            - sp2
//
//
//__________________________________________________________
// hardware:
// ---------
// lego nxt
// usb kabel
//
//__________________________________________________________
// anschlussbelegung:
// ------------------
// OUT_1    - motor links
// OUT_2    - nicht belegt
// OUT_3    - motor rechts
// SENSOR_1 - optischer sensor(lego-licht-sensor) links
// SENSOR_2 - optischer Sensor(lego-licht-sensor) mitte
// SENSOR_3 - optischer Sensor(lego-licht-sensor) rechts
// SENSOR_4 - nicht belegt
//
//__________________________________________________________
// programmablauf/doku:
// --------------------
//
//
//  1.) datei "NXCDefs.h" includieren.
//
//
//  2.) variablen deklaration:DunkelL    = oberer grenzwert 
//					   links
//                            DunkelM    = oberer grenzwert
//					   mitte
//                            DunkelR    = oberer grenzwert
//					   rechts
//                            HellL      = unterer grenzwert
//					   links
//                            HellR      = unterer grenzwert
//					   mitte
//                            HellM      = unterer grenzwert
//					   rechts
//                            iL         = sensorwert von 
//					   SENSOR_1
//                            iM         = sensorwert von
//					   SENSOR_2
//                            iR         = sensorwert von
//					   SENSOR_3
//                            Tolleranz  = 1/5 der diff von
//					   hell und dunkel
//                            Kraft      = motorkraft
//                            fM         = menuesteuerung
//                            i          = hilfsvariable
//                            BtnL links = button 2
//                            BtnR rechts= button 1
//                            BtnE Enter = button 3
//
//  3.) makro-definitionen.
//
//
//  4.) variablen werden initialisiert.
//
//
//  5.) optische sensoren an eingang 1, 2 und 3 mit raw modus
//	bekannt machen.
//
//
//  6.) hauptmenue anzeigen(gehe zu inputmenue um grenzwerte
//	einzulesen).
//
//
//  7.) das unterprogramm hauptmenue() erlaubt dem benutzer 
//	zwischen den unterprogrammen Liniefolgen(), 
//	InputMenue(), WerteTabelle(), EnergieAnzeige(), 
//      SetTolleranz() und SetKraft() zu verzweigen.
//      fM1   >> Liniefolgen()
//      fM2   >> InputMenue()
//      fM3   >> WerteTabelle()
//      fM4   >> EnergieAnzeige()
//      fM5   >> SetTolleranz()
//      fM6   >> SetKraft()
//
//
//  8.) im task Liniefolgen() befindet sich der 
//	eigentliche algorithmus zum linie entlang fahren.
//	dieser teilt sich in folgende sieben aufgaben auf:
//
//
//      + fahrauftrag bestimmen
//      -----------------------
//      um eine hohe durchschnittsgeschwindigkeit beibehalten
//	zu koennen sollten sich moeglichst immer beide raeder 
//	drehen. als erstes werden durch den aufruf des
//	unterprogramms SW() die sensorwerte aktualisiert.
//      nun kann durch den vergleich der werte der neue 
//	fahrauftrag bestimmt werden. befindet sich der
//      mittlere sensor allein ueber der linie so wird das
//      unterprogramm GeradeF() ausgefuehrt. Kommt das 
//	fahrzeug nun zum linken rand der linie(dass heisst 
//	der mittlere und der rechte sensor "sehen" die Linie),
//	so wird mit einem einfachen gegensteuern durch den 
//	aufruf des unterprogramms RechtsF() gegengelenkt. 
//	wenn aber der mittlere sensor die linie verlassen 
//	hat(zum beispiel nach rechts(nur der linke sensor
//      "sieht" die Linie noch)) dann wird mit dem 
//	Unterprogramm RechtsD() stark gegengelenkt.
//
//
//      +-------------+-----------+-----------+-----------+
//      | fahrauftrag | SENSOR_1  | SENSOR_2  | SENSOR_3  |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | Stop()      | 0         | 0         | 0         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | RechtsD()   | 0         | 0         | 1         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | GeradeF()   | 0         | 1         | 0         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | RechtsF()   | 0         | 1         | 1         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | LinksD()    | 1         | 0         | 0         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | Stop()      | 1         | 0         | 1         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | LinksF()    | 1         | 1         | 0         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//      | Anhalten()  | 1         | 1         | 1         |
//      |             |           |           |           |
//      +-------------+-----------+-----------+-----------+
//
//	0 = hell
//      1 = dunkel
//
//
//      + makro GeradeF() -ahren
//      ------------------------
//      linker und rechter motor drehen mit "Kraft1"
//	vorwaerts.
//
//
//      + makro LinksF() -ahren
//      -----------------------
//      linker motor dreht mit "Kraft2", 
//	rechter mit "Kraft1" vorwaerts.
//
//
//      + makro RechtsF() -ahren
//      ------------------------
//      rechter motor dreht mit "Kraft2",
//	linker mit "Kraft1" vorwaerts.
//
//
//      + makro LinksD() -rehen
//      -----------------------
//      linker motor dreht mit "Kraft2" rueckwaerts. 
//	rechter motor mit "Kraft1" vorwaerts.
//
//
//      + makro RechtsD() -rehen
//      ------------------------
//      rechter motor dreht mit "Kraft2" rueckwaerts. 
//	linker motor mit "Kraft1" vorwaerts.
//
//      + makro Anhalten()
//      ------------------
//      linker und rechter motor stoppen.
//
//
//  9.) der task InputMenue() beinhaltet 
//	die grenzwerterfassung der optischen sensoren.
//      beschreibung der grenzwerterfassung:
//      variablen "DunkelL", "DunkelM", "DunkelR",
//	"HellL", "HellM" und "HellR" mit werten 
//	belegen. 
//	dazu wird das fahrzeug <ueber> die schwarze
//	linie bewegt. zum speichern der werte genuegt
//	ein druck des orangenen tasters am nxt und 
//	man befindet sich wieder im hauptmenue.
//
//
// 10.) der task WerteTabelle(), ruft zur 
//	aktuallisierung der sensorwerte, in einem 
//	8000 millisekunden interval das unterprogramm 
//	SW() auf. dann zeigt es alle wichtigen werte 
//	des programms in einer tabelle auf dem 
//	display. dabei bedeutet:
//
//      Sensor = aktueller wert
//      Hell   = wert von variable "Hell"(L, M, R)
//      Dark   = wert von variable "Dunkel"(L, M, R)
//      L      = linker sensor(1)
//      M      = mittlerer sensor(2)
//      R      = rechter sensor(3)
//                           
//      Sensor Hell Dark     
//         
//      L=0000 0000 0000      
//           
//      M=0000 0000 0000         
//     
//      R=0000 0000 0000
//
//
//      merke: je heller, desto groesser der wert.
//
//      um zum hauptmenue zurueck zu kehren drueckt
//	man einfach den orangenen Button.
//
//
// 11.) den aktuellen ladezustand des akku erfaehrt
//	man im task EnergieAnzeige().
//      um zum hauptmenue wieder zurueck zu kehren drueckt
//	man einfach den orangenen Button.
//
//
// 12.) den wert der variblen tolleranz aendert man im 
//	task SetTolleranz(). dies geschieht durch
//	einfaches betaetigen der pfeieltasten.
//      zurueck zum hauptmenue gehts mit dem orangenen Button.
//
//
// 13.) die drehkraft der motoren wird im task SetKraft() 
//	mit den pfeieltasten eingestellt.
//	der orangene Button bringt dich wie immer zurueck ins 
//	hauptmenue.
//
//
// 14.) im unterprogramm SW() werden die aktuellen 
//	sensorwerte in die dazugehoerigen variablen
//	geschrieben.
//      beispiel: "iL" = sensorwert des linken sensor
//__________________________________________________________

#include "NXCDefs.h"

int DunkelL;
int DunkelM;
int DunkelR;
int HellL;
int HellM;
int HellR;
int iL;
int iM;
int iR;
bool BtnL;
bool BtnR;
bool BtnE;
byte Kraft1;
byte Kraft2;
byte Tolleranz;
byte fM;
bool fM1;
bool fM2;
bool fM3;
bool fM4;
bool fM5;
bool fM6;
byte i;
byte k; // aktueller Fahrbefehl
byte r; // zuletzt gefahrene Richtung(entspricht dem k-Wert(ausser alle sind Hell oder Dunkel))
string LF = "Liniefolgen";
string IM = "InputMenue";
string WT = "WerteTabelle";
string EA = "EnergieAnzeige";
string ST = "Set Tolleranz";
string SK = "Set Kraft";
string tmp;
bool ea; // wenn Energieabfrage schon statt gefunden hat
bool ia; // wenn Grenzwerterfassung schon im Untermenue ausgegeben wurde
#define ButtonL ButtonState(2);
#define ButtonR ButtonState(1);
#define ButtonE ButtonState(3);
#define ButtonPause Wait(1200);

#define MotorLv100 OnFwd (OUT_A,Kraft1);
#define MotorLvHalb OnFwd (OUT_A,Kraft2);
#define MotorLz100 OnRev (OUT_A,Kraft1);
#define MotorLzHalb OnRev (OUT_A,Kraft2);
#define MotorLstop Off (OUT_A);
#define MotorRv100 OnFwd (OUT_C,Kraft1);
#define MotorRvHalb OnFwd (OUT_C,Kraft2);
#define MotorRz100 OnRev (OUT_C,Kraft1);
#define MotorRzHalb OnRev (OUT_C,Kraft2);
#define MotorRstop Off (OUT_C);

#define GeradeF   OnFwd (OUT_A,Kraft1 + 10);  \
                  OnFwd (OUT_C,Kraft1 + 10);

#define GeradeZ   MotorLzHalb  \
                  MotorRzHalb;

#define LinksF    MotorLvHalb; \
                  MotorRv100;
               
#define RechtsF   MotorLv100;  \
                  MotorRvHalb;
                 
#define LinksD    MotorLz100;  \
                  MotorRv100;
                 
#define RechtsD   MotorLv100;  \
                  MotorRz100;

#define Anhalten  MotorLstop;  \
                  MotorRstop;

sub SW()
{
iL = SENSOR_1;
iM = SENSOR_2;
iR = SENSOR_3;
}

void ScrollBarH(byte v)
{
byte xL1;
byte xL2;
byte xR1;
byte xR2;
byte l;
byte m = 30;
byte sw = 10;
v = v - 1;
  if(v > 0)
  {
  l = 10;
  }
  else
  {
  l = 9;
  }
xL1 = l + (v * sw)
xL2 = xL1 + 1
xR1 = xL1 + m
xR2 = xR1 - 1
LineOut(1,0,98,0);
LineOut(0,1,0,5);
LineOut(3,3,5,1);
LineOut(4,3,6,1);
LineOut(3,3,5,5);
LineOut(4,3,6,5);
LineOut(8,0,8,6);
LineOut(xL2,1,xR2,1);
LineOut(xL1,2,xL1,4);
LineOut(xR1,2,xR1,4);
LineOut(xL2,5,xR2,5);
LineOut(91,0,91,6);
LineOut(94,1,96,3);
LineOut(93,1,95,3);
LineOut(94,5,96,3);
LineOut(93,5,95,3);
LineOut(99,1,99,5);
LineOut(1,6,98,6);
}

sub MenueOut()
{
LineOut(0,55,99,55);
  switch(fM)
  {
  case 1:
  tmp = LF;
  break;
  case 2:
  tmp = IM;
  break;
  case 3:
  tmp = WT;
  break;
  case 4:
  tmp = EA;
  break;
  case 5:
  tmp = ST;
  break;
  case 6:
  tmp = SK;
  break;
  }
}

sub HauptMenue()
{
ClearScreen();
ScrollBarH(fM);
MenueOut();
TextOut(10, LCD_LINE6, tmp);
TextOut(5, LCD_LINE4, "gehst Du zu");
             if(fM == 1 || fM == 5 ||fM == 6)
             {
             tmp = "...";
             }
               if(fM == 2)
               {
               tmp = "m...";
               }
                 if(fM == 3 || fM == 4)
                 {
                 tmp = "r...";
                 }
TextOut(71, LCD_LINE4,tmp);
TextOut(5, LCD_LINE3, "Mit Enter");
TextOut(21, LCD_LINE1, "Hauptmenue");
}

sub UnterMenue()
{
ClearScreen();
TextOut(5, LCD_LINE8, "der Entertaste!");
TextOut(5, LCD_LINE7, "Beenden mit");
MenueOut();
TextOut(7, LCD_LINE1, tmp);
}

sub LinieSuchen()
{
PlayTone(444,100);
  switch(r)
  {
  case 10:      // vorher gerade gefahren
  GeradeZ;
  break;
  case 11:      // vorher links gefahren
  LinksD;
  break;
  case 1:       // vorher links gedreht
  LinksD;
  break;
  case 110:     // vorher rechts gefahren
  RechtsD;
  break;
  case 100:     // vorher rechts gedreht
  RechtsD;
  break;
  }
Wait(100);
}

task LinieFolgen()
{
  while(true)
  {
    if(fM1)
    {
    k = 0;
    SW();
    ClearScreen();
      if(iL <= DunkelL + Tolleranz)
      {
      k = k + 1;
      }
        if(iM <= DunkelM + Tolleranz)
        {
        k = k + 10;
        }
          if(iR <= DunkelR + Tolleranz)
          {
          k = k + 100;
          }
            if(! k == 0 && ! k == 111)
            {
            r = k;
            }
      switch(k)
      {
      case 0:
      tmp = "Anhalten Hell";
      LinieSuchen();
      break;
      case 1:
      tmp = "links drehen";
      LinksD;
      break;
      case 10:
      tmp = "gerade fahren";
      GeradeF;
      break;
      case 11:
      tmp = "links fahren";
      LinksF;
      break;
      case 100:
      tmp = "rechts drehen";
      RechtsD;
      break;
      case 110:
      tmp = "rechts fahren";
      RechtsF;
      break;
      case 111:
      tmp = "Anhalten Dunkel";
      LinieSuchen();
      break;
      }
    TextOut(0, LCD_LINE8, tmp);
    }
  }
}

task InputMenue()
{
  while(true)
  {
    if(fM2)
    {
    SW();
      if(HellL < iL)
      {
      HellL = iL;
      }
        if(DunkelL > iL)
        {
        DunkelL = iL;
        }
          if(HellM < iM)
          {
          HellM = iM;
          }
            if(DunkelM > iM)
            {
            DunkelM = iM;
            }
              if(HellR < iR)
              {
              HellR = iR;
              }
                if(DunkelR > iR)
                {
                DunkelR = iR;
                }
                  if(!ia)
                  {
                  TextOut(5, LCD_LINE4, "werden gelesen");
                  TextOut(5, LCD_LINE3, "Grenzwerte");
                  ia = true;
                  }
    }
  }
}

task WerteTabelle()
{
  while(true)
  {
    if(fM3)
    {
    SW();
    ClearScreen();
    NumOut(72, LCD_LINE8,  DunkelR);
    NumOut(42, LCD_LINE8,  HellR);
    NumOut(12, LCD_LINE8,  iR);
    TextOut(0, LCD_LINE8, "R=");
    NumOut(72, LCD_LINE6,  DunkelM);
    NumOut(42, LCD_LINE6,  HellM);
    NumOut(12, LCD_LINE6,  iM);
    TextOut(0, LCD_LINE6, "M=");
    NumOut(72, LCD_LINE4,  DunkelL);
    NumOut(42, LCD_LINE4,  HellL);
    NumOut(12, LCD_LINE4,  iL);
    TextOut(0, LCD_LINE4, "L=");
    TextOut(0, LCD_LINE2, "Sensor Hell Dark");
    Wait(6000);
    UnterMenue();
    TextOut(5, LCD_LINE4, "aktuelle Werte");
    Wait(2000);
    }
  }
}

task EnergieAnzeige()
{
int e;
  while(true)
  {
    if(fM4)
    {
      if(!ea)
      {
      UnterMenue();
      e = BatteryLevel();
      TextOut(5, LCD_LINE4, "Energie=");
      NumOut(55,LCD_LINE4,e);
      TextOut(81, LCD_LINE4, "mV");
      ea = true;
      }
    }
  }
}

task SetTolleranz()
{
  while(true)
  {
    if(fM5)
    {
    UnterMenue();
    TextOut(5, LCD_LINE4, "Tolleranz=");
    NumOut(66,LCD_LINE4,Tolleranz);
    Wait(1000);
    }
  }
}

task SetKraft()
{
  while(true)
  {
    if(fM6)
    {
    UnterMenue();
    TextOut(5, LCD_LINE4, "Kraft=");
    NumOut(42,LCD_LINE4,Kraft1);
    Wait(300);
    }
  }
}

task main()
{
SetSensorLight(S1);
SetSensorLight(S2);
SetSensorLight(S3);
SetSensorMode(S1, SENSOR_MODE_RAW);
SetSensorMode(S2, SENSOR_MODE_RAW);
SetSensorMode(S3, SENSOR_MODE_RAW);
SetSoundVolume(5);
iL = 0;
iM = 0;
iR = 0;
Kraft1 = 40;
Kraft2 = 10;
fM1 = false;
fM2 = false;
fM3 = false;
fM4 = false;
fM5 = false;
fM6 = false;
Tolleranz = 100;
start LinieFolgen;
start InputMenue;
start WerteTabelle;
start EnergieAnzeige;
start SetTolleranz;
start SetKraft;
fM = 2;
HauptMenue();
  while(true)
  {
  BtnL = ButtonL;
  BtnR = ButtonR;
  BtnE = ButtonE;
      if(BtnL)
      {
        if(! fM5 && ! fM6)
        {
          if(fM > 1)
          {
          fM = fM - 1;
          HauptMenue();
          }
        }
        else
        {
          if(fM5)
          {
            if(Tolleranz > 4)
            {
            Tolleranz = Tolleranz - 10
            }
          }
          if(fM6)
          {
            if(Kraft1 > 4)
            {
            Kraft1 = Kraft1 - 4
            Kraft2 = Kraft1 / 4
            }
          }
        }
      ButtonPause;
      }
      if(BtnR)
      {
        if(! fM5 && ! fM6)
        {
          if(fM < 6)
          {
          fM = fM + 1;
          HauptMenue();
          }
        }
        else
        {
          if(fM5)
          {
            if(Tolleranz < 251)
            {
            Tolleranz = Tolleranz + 10
            }
          }
          if(fM6)
          {
            if(Kraft1 < 85)
            {
            Kraft1 = Kraft1 + 4
            Kraft2 = Kraft1 / 4
            }
          }
        }
      ButtonPause;
      }
      if(BtnE)
      {
        switch(fM)
        {
        case 1:
          if(fM1)
          {
          fM1 = false;
          Anhalten;
          HauptMenue();
          }
          else
          {
          fM1 = true;
          }
        break;
        case 2:
          if(fM2)
          {
          fM2 = false;
          HauptMenue();
          }
          else
          {
          DunkelL = 1000;
          DunkelM = 1000;
          DunkelR = 1000;
          HellL = 0;
          HellM = 0;
          HellR = 0;
          UnterMenue();
          ia = false;
          fM2 = true;
          }
        break;
        case 3:
          if(fM3)
          {
          fM3 = false;
          HauptMenue();
          }
          else
          {
          fM3 = true;
          }
        break;
        case 4:
          if(fM4)
          {
          fM4 = false;
          HauptMenue();
          }
          else
          {
          ea = false;
          fM4 = true;
          }
        break;
        case 5:
          if(fM5)
          {
          fM5 = false;
          HauptMenue();
          }
          else
          {
          fM5 = true;
          }
        break;
        case 6:
          if(fM6)
          {
          fM6 = false;
          HauptMenue();
          }
          else
          {
          fM6 = true;
          }
        break;
        }
      ButtonPause;
      }
  }
}