basketball9

leggor.de -> ris -> basketball

<< >>

// software:
// ---------
// compiler:  nqc.exe               - version 3.1.2.5
// ide:       bricx command center  - version 3.3
// firmware:  lego                  - version 0309
// os:        windows xp            - sp2
//
//
// _________________________________________________________
// hardware:
// ---------
// serieller tower
// sitecom usb to serial
// rcx 1.0
//
//
// _________________________________________________________
// anschlussbelegung:
// ------------------
// OUT_A    - motor (ventil  fwd-hand/rev-arm)
// OUT_B    - motor (kompressor)
// OUT_C    - motor (arm drehen  fwd-links / rev-rechts)
// SENSOR_1 - optischer sensor(hand wenn hell-offen)
// SENSOR_2 - optischer sensor(arm hell-oben)
// SENSOR_3 - schliesser
//
//
// _________________________________________________________
// programm:
// ---------

// vriablendeklaration (global)_____________________________
int In1;            // wert an SENSOR_1
int In2;            // wert an SENSOR_2
int DH;             // dunkel hand
int HH;             // hell hand
int DA;             // dunkel arm
int HA;             // hell arm
int DZ;             // zeit die der arm beim drehen braucht, 
                    // bis in er sich in einer der zwei 
                    // definierten stellungen befindet
int K;              // kalibrierung(bool) / hilfsvariable 
                    // beim kalibrieren der sensoren
int KT;             // zeitintervall der kalibrierung
int L;              // luft(bool)
int T;              // tolleranz
int i;              // schleifenzaehler
int Kraft;          // motorpower

// macrodefinition _________________________________________
#define Freq 440    // akustische fehlermeldung

// programmstart ___________________________________________
task main()
{
SetSensorType(SENSOR_1,SENSOR_TYPE_LIGHT);
SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
SetSensorType(SENSOR_2,SENSOR_TYPE_LIGHT);
SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
SetSensorType(SENSOR_3,SENSOR_TYPE_TOUCH);
SetSensorMode(SENSOR_3,SENSOR_MODE_BOOL);
DZ = 200;
K = 1;
KT = 800;
L = 0;
T = 20;
Kraft = 5;
SetPower(OUT_A, Kraft);
start Kalib;
OnFwd(OUT_A);
Wait(KT);
Off(OUT_A);
OnRev(OUT_A);
Wait(KT);
Off(OUT_A);
K = 0;
stop Kalib;
// Grenzwertberechnung
start Luft;
DH = DH - T;
HH = HH + T;
DA = DA - T;
HA = HA + T;
GS();
  while(true)
  {
  BW();
  }
}

// grenzwerterfassung der sensoren _________________________
task Kalib()
{
  while(K == 1)
  {
  IN();
    if(In1 < HH)
    {
    HH = In1;
    }
      if(In1 > DH)
      {
      DH = In1;
      }
        if(In2 < HA)
        {
        HA = In2;
        }
          if(In2 > DA)
          {
          DA = In2;
          }
  }
}

// kompressor ______________________________________________
task Luft()
{
  while(true)
  {
  L = SENSOR_3;
    if(! L)
    {
    OnRev(OUT_B);
    }
    else
    {
    Off(OUT_B);
    }
  }
}

// grundstellung ___________________________________________
void GS()
{
Hauf();
Aheben();
Rechts();
}

// ball werfen _____________________________________________
void BW()
{
Asenken();
Hzu();
Aheben();
Links();
Hauf();
Rechts();
}

// hand oeffnen ____________________________________________
void Hauf()
{
In1 = SENSOR_1;
  if(In1 <= HH)    // hand ist bereits geoeffnet 
                   // (sensor= hell)
  {
    repeat(2)
    {
    Fehler();
    }
  }
  else
  {
  OnFwd(OUT_A);
    while(In1 >= HH)// so lange wie sensor hell ist ventil 
                    // schalten dann ventil stop
    {
    In1 = SENSOR_1;
    }
  Off(OUT_A);
  }
}

// hand schliessen _________________________________________
void Hzu()
{
In1 = SENSOR_1;
  if(In1 >= DH)    // hand ist bereits geschlossen
                   // (sensor= hell)
  {
    repeat(2)
    {
    Fehler();
    }
  }
  else
  {
  OnFwd(OUT_A);
    while(In1 <= DH)// so lange wie sensor hell ist ventil
                    // schalten dann ventil stop
    {
    In1 = SENSOR_1;
    }
  Off(OUT_A);
  }
}

// arm nach links drehen ___________________________________
sub Links()
{
OnFwd(OUT_C);
Wait(DZ);
Off(OUT_C);
}

// arm nach rechts drehen __________________________________
sub Rechts()
{
OnRev(OUT_C);
Wait(DZ);
Off(OUT_C);
}

// arm heben _______________________________________________
void Aheben()
{
In2 = SENSOR_2;
  if(In2 >= DA)    // arm ist bereits angehoben 
                   // (sensor=dunkel)
  {
    repeat(2)
    {
    Fehler();
    }
  }
  else
  {
  OnRev(OUT_A);
    while(In2 < DA)// so lange wie sensor hell ist ventil 
                   // schalten dann ventil stop
    {
    In2 = SENSOR_2;
    }
  Off(OUT_A);
  }
}

// arm senken ______________________________________________
void Asenken()
{
In2 = SENSOR_2;
  if(In2 <= HA)    // arm ist bereits unten (sensor= hell)
  {
    repeat(2)
    {
    Fehler();
    }
  }
  else
  {
  OnRev(OUT_A);
    while(In2 > HA)// so lange wie sensor dunkel ist ventil 
                   // schalten dann ventil stop
    {
    In2 = SENSOR_2;
    }
  Off(OUT_A);
  }
}

// optische sensoren abfragen ______________________________
sub IN()
{
In1 = SENSOR_1;
In2 = SENSOR_2;
}

// akustische fehlerberichterstattung ______________________
sub Fehler()
{
Wait(200);
PlayTone(Freq,100);
}