//#################################################################################
// Program: Gepard
// Copyright: 2011 Robert Rudloff
// jede komerzielle Nutzung (auch Auszugsweise) erfordet die ausdrückliche und schriftliche Genehmigung durch den Autor !!!!
//#################################################################################
import stamp.peripheral.io.I2C ;
import stamp.core.CPU ;
import stamp.peripheral.io.SD21 ;
import stamp.peripheral.io.TMC222 ;
/*____________________________________________________________________________*/
public class gepard
{
 final static char I2CADR_TMC222_TurmDrehen  = 0xC0 ;
 final static char I2CADR_TMC222_TurmHeben = 0xC4 ;

 final static byte ADR_FolgeradarDrehen       =  1 ; /*Ok!*/
 final static byte ADR_FolgeradarHeben        =  2 ; /*Ok!*/
 final static byte ADR_PeriskopLinksDrehen    =  4 ; /*Ok!*/
 final static byte ADR_PeriskopRechtsDrehen   =  5 ; /*Ok!*/
 final static byte ADR_TurmlukeHeben          =  6 ; /*Ok!*/
 final static byte ADR_SuchradarDrehen        =  7 ; /*Ok!*/
 final static byte ADR_SuchradarHeben         =  8 ; /*Ok!*/
 final static byte ADR_SoundGas               =  9 ; /*Ok!*/
 final static byte ADR_SoundKey               = 10 ; /*Ok!*/


 final static byte POS_SoundGasStandgas       = 100 ; /*Ok!*/
 final static byte POS_SoundGasVollgas        = 120 ; /*Ok!*/
 final static byte POS_SoundKeyS0             =   0 ; /*Ok!*/
 final static byte POS_SoundKeyS1             =  10 ; /*Ok!*/
 final static byte POS_SoundKeyS2             =  20 ; /*Ok!*/
 final static byte POS_SoundKeyS3             =  30 ; /*Ok!*/
 final static byte POS_SoundKeyS4             =  40 ; /*Ok!*/
 final static byte POS_SoundKeyS5             =  50 ; /*Ok!*/

 final static byte POS_SoundMotorAnAus        =  POS_SoundKeyS1 ; /*Ok!*/
 final static byte POS_SoundMotorZwischengas  =  POS_SoundKeyS2 ; /*Ok!*/
 final static byte POS_SoundWaffeFeuer        =  POS_SoundKeyS5 ; /*Ok!*/


 final static int POSSET_FolgeradarDrehenCenter = 105 ; //Ok!   105
 final static int POS_FolgeradarDrehenCenter    =   0 ; //Ok!     0
 final static int POS_FolgeradarDrehenLMax      = 174 ; //Ok!   174
 final static int POS_FolgeradarDrehenRMax      = -160 ; //Ok! -160
 final static int SPEED_FolgeradarDrehenMax     =   30 ; //Ok!   30

 final static int POS_FolgeradarHebenCenter   = 95 ;
 final static int POS_FolgeradarHebenObenMax  = 240 ;
 final static int POS_FolgeradarHebenUntenMax = 72 ;

 final static int POS_SuchradarOben           = 225 ; /*Ok!*/
 final static int POS_SuchradarUnten          =  73 ; /*Ok!*/
 final static int POS_SuchradarAn             = 130 ; /*Ok!*/
 final static int POS_SuchradarLangsam        = 121 ; /*Ok!*/
 final static int POS_SuchradarAus            = 117 ; /*Ok !  117 = aus*/
 final static int SPEED_SuchradarHoch         = 15  ; /*Ok!*/

 final static int POS_TurmlukeAuf             = 65  ; /*Ok!*/
 final static int POS_TurmlukeAufMax          = 53  ; /*Ok!*/
 final static int POS_TurmlukeZu              = 230  ; /*Ok!*/
 final static int POS_TurmlukeFastZu          = 215  ; /*Ok!*/

 final static int POS_PeriskopLinksLMax       = -40 ; /*Ok!*/
 final static int POS_PeriskopLinksRMax       = 250 ; /*Ok???*/
 final static int POS_PeriskopLinksCenter     = 100 ;
 final static int POS_PeriskopLinksL90        = 120 ;
 final static int POS_PeriskopLinksR90        = 120 ;

 final static int POS_PeriskopRechtsLMax      = -30 ; /*Ok!*/
 final static int POS_PeriskopRechtsRMax      = 250 ; /*Ok???*/
 final static int POS_PeriskopRechtsCenter    = 100 ;
 final static int POS_PeriskopRechtsL90       = 120 ;
 final static int POS_PeriskopRechtsR90       = 120 ;

 private static I2C SharedI2CBus ;
 private static SD21 SD21 ;
 private static TMC222 TurmDrehen ;
 private static TMC222 TurmHeben  ;


 //_____________________________________________________________________________
 private static void SoundTSBProgrammieren ()
 {SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  System.out.println("Start");
  Pause(10);
  System.out.println("S1");
  SD21.SetPos(ADR_SoundKey,POS_SoundKeyS1); Pause(1); SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  Pause(10);
  System.out.println("S2");
  SD21.SetPos(ADR_SoundKey,POS_SoundKeyS2); Pause(1); SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  Pause(10);
  System.out.println("S3");
  SD21.SetPos(ADR_SoundKey,POS_SoundKeyS3); Pause(1); SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  Pause(10);
  System.out.println("S4");
  SD21.SetPos(ADR_SoundKey,POS_SoundKeyS4); Pause(1); SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  Pause(10);
  System.out.println("S5");
  SD21.SetPos(ADR_SoundKey,POS_SoundKeyS5); Pause(1); SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  Pause(10);
 }
 //_____________________________________________________________________________
 private static void Sound (byte POS_Sound)
 {SD21.SetPos(ADR_SoundKey,POS_Sound);
  Pause(0,5);
  SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
 }
 //_____________________________________________________________________________
 private static void Pause (int Min, int Sec, int Tics )
 {Tics += Sec * 10 ;
  Tics += Min * 600 ;
  for(Tics = Tics; Tics > 0 ; Tics --)
  {CPU.delay(1000);
  }
 }
 //_____________________________________________________________________________
 private static void Pause (int Sec, int Tics )
 {Pause(0,Sec,Tics);
 }
 //_____________________________________________________________________________
 private static void Pause (int Sec)
 {Pause(0,Sec,0);
 }
 //_____________________________________________________________________________
 private static void SD21_FailSave ()
 {SD21.SetCenter(ADR_FolgeradarDrehen ,POSSET_FolgeradarDrehenCenter);
  SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
  SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
  SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,0);
  SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
  SD21.SetPos(ADR_SuchradarHeben      ,POS_SuchradarOben         ,0);
  SD21.SetPos(ADR_SuchradarDrehen     ,POS_SuchradarAus          ,0);
  SD21.SetPos(ADR_TurmlukeHeben       ,POS_TurmlukeZu            ,0);
  SD21.SetPos(ADR_SoundGas            ,POS_SoundGasStandgas      ,0);
  SD21.SetPos(ADR_SoundKey            ,POS_SoundKeyS0            ,0);
 }
 //_____________________________________________________________________________
 private static void Grundstellung (int Sekunden)
 {SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
  SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
  SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
  SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
  SD21.SetPos(ADR_SuchradarHeben      ,POS_SuchradarOben         ,0);
  SD21.SetPos(ADR_SuchradarDrehen     ,POS_SuchradarAus          ,0);
  SD21.SetPos(ADR_TurmlukeHeben       ,POS_TurmlukeZu            ,0);
  SD21.SetPos(ADR_SoundGas            ,POS_SoundGasStandgas      ,0);
  SD21.SetPos(ADR_SoundKey            ,POS_SoundKeyS0            ,0);
  TurmHeben.SetPosition(0);
  TurmDrehen.SetPosition(0);
  Pause(Sekunden);
 }
 //_____________________________________________________________________________
 private static void Zigarettenpause (int Sekunden)
 {TurmHeben.SetPosition(0);
  TurmDrehen.SetPosition(0);
  SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
  SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
  SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
  SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
  Pause(1);
  SD21.SetPos(ADR_SuchradarHeben ,POS_SuchradarOben    ,0);
  SD21.SetPos(ADR_SuchradarDrehen,POS_SuchradarLangsam ,0);
  Pause(1);
  SD21.SetPos(ADR_SuchradarDrehen,POS_SuchradarAus ,0);
  Pause(5);
  SD21.SetPos(ADR_TurmlukeHeben  ,POS_TurmlukeAuf  ,0);
  Pause(Sekunden);
 }
 //_____________________________________________________________________________
 private static void LuftraumUberwachung (int Sekunden)
 {SD21.SetPos(ADR_TurmlukeHeben  ,POS_TurmlukeZu  ,0);
  Pause(3);
  SD21.SetPos(ADR_SuchradarHeben ,POS_SuchradarOben    ,0);
  Pause(2);
  SD21.SetPos(ADR_SuchradarDrehen,POS_SuchradarAn ,0);
  Pause(1);
  TurmHeben.SetPosition(0);
  TurmDrehen.SetPosition(0);
  SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
  SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
  SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
  SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);


  Pause(Sekunden);
 }
 //_____________________________________________________________________________
 private static void ZielerfassungR1 (boolean Erfassung, boolean Verfolgung, boolean Feuer,boolean Absturz,boolean Grundstellung)
 {int i ;
  int PosTurmHeben;
  int PosTurmDrehen;
  PosTurmHeben  =   400 ;
  PosTurmDrehen = -1000 ;
  // Folgeradar: ausfahren
  SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
  SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenLMax  - 8,SPEED_FolgeradarDrehenMax-10);
  Pause(0,4);
  TurmHeben.SetPosition(PosTurmHeben );
  TurmDrehen.SetPosition(PosTurmDrehen);

  // Folgeradar: Zielerfassung
  Pause(0,5);
  if(Erfassung)
  {for(i=1;i<=2;i++)
   {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
    Pause(0,3);
    SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +50,10);
    Pause(0,3);
   }
  }
  //Zielverfolgung
  if(Verfolgung)
  {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +30,10);
   SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenLMax     ,SPEED_FolgeradarDrehenMax);
   PosTurmDrehen -= 90 ;
   TurmDrehen.SetPosition(PosTurmDrehen);
   Pause(0,5);
   for(i=1;i<500;i+=1)
   {PosTurmDrehen -= 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
   }
  }
  //Zielbekämpfung
  if(Feuer)
  {SD21.SetPos(ADR_SoundKey,POS_SoundWaffeFeuer);
   for(i=1;i<200;i+=1)
   {int x = 0;
    PosTurmDrehen -= 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    /*
    if(x == 0)       {TurmHeben.SetPosition(450);    }
    else if( x == 20) {TurmHeben.SetPosition(300);    }
    else if( x == 40) {TurmHeben.SetPosition(500);    }
    else if( x == 60) {TurmHeben.SetPosition(300);    }
    else if( x == 80) {TurmHeben.SetPosition(500);    }
    else if( x == 100) {TurmHeben.SetPosition(300);    }
    else if( x == 120) {TurmHeben.SetPosition(500);    }
    else if( x == 140) {TurmHeben.SetPosition(300);    }
    else if( x == 160) {TurmHeben.SetPosition(500);    }
    else if( x == 180) {TurmHeben.SetPosition(400);    }
    */
   }
   SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  }
  //Zielbekämpfung
  if(Absturz)
  {for(i=1;i<400;i+=1)
   {int x = 0;
    PosTurmDrehen -= 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    PosTurmHeben -= 1 ;
    TurmHeben.SetPosition(PosTurmHeben);
   }
  }
  // Folgeradar Zurücksetzen
  if(Grundstellung)
  {TurmHeben.SetPosition(0);
   TurmDrehen.SetPosition(0);
   SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
   SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
   SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
   SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
   Pause(1);
  }
 }
 //_____________________________________________________________________________
 private static void ZielerfassungR2 (boolean Erfassung, boolean Verfolgung, boolean Feuer,boolean Absturz,boolean Grundstellung)
 {int i ;
  int PosTurmHeben;
  int PosTurmDrehen;
  PosTurmHeben  =   600 ;
  PosTurmDrehen = -3000 ;
  // Folgeradar: ausfahren
  SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
  SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenLMax  - 8,SPEED_FolgeradarDrehenMax-10);
  Pause(0,4);
  TurmHeben.SetPosition(PosTurmHeben );
  TurmDrehen.SetPosition(PosTurmDrehen);

  // Folgeradar: Zielerfassung
  Pause(0,5);
  if(Erfassung)
  {for(i=1;i<=2;i++)
   {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
    Pause(0,3);
    SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +50,10);
    Pause(0,3);
   }
  }
  //Zielverfolgung
  if(Verfolgung)
  {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +30,10);
   SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenLMax     ,SPEED_FolgeradarDrehenMax);
   PosTurmDrehen -= 90 ;
   TurmDrehen.SetPosition(PosTurmDrehen);
   Pause(0,5);
   for(i=1;i<500;i+=1)
   {PosTurmDrehen += 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
   }
  }
  //Zielbekämpfung
  if(Feuer)
  {SD21.SetPos(ADR_SoundKey,POS_SoundWaffeFeuer);
   for(i=1;i<600;i+=1)
   {int x = 0;
    PosTurmDrehen += 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    /*
    if(x == 0)       {TurmHeben.SetPosition(450);    }
    else if( x == 20) {TurmHeben.SetPosition(300);    }
    else if( x == 40) {TurmHeben.SetPosition(500);    }
    else if( x == 60) {TurmHeben.SetPosition(300);    }
    else if( x == 80) {TurmHeben.SetPosition(500);    }
    else if( x == 100) {TurmHeben.SetPosition(300);    }
    else if( x == 120) {TurmHeben.SetPosition(500);    }
    else if( x == 140) {TurmHeben.SetPosition(300);    }
    else if( x == 160) {TurmHeben.SetPosition(500);    }
    else if( x == 180) {TurmHeben.SetPosition(400);    }
    */
   }
   SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  }
  //Zielbekämpfung
  if(Absturz)
  {for(i=1;i<400;i+=1)
   {int x = 0;
    PosTurmDrehen += 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    PosTurmHeben -= 1 ;
    TurmHeben.SetPosition(PosTurmHeben);
   }
  }
  // Folgeradar Zurücksetzen
  if(Grundstellung)
  {TurmHeben.SetPosition(0);
   TurmDrehen.SetPosition(0);
   SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
   SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
   SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
   SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
   Pause(1);
  }
 }
 //_____________________________________________________________________________
 private static void ZielerfassungL1 (boolean Erfassung, boolean Verfolgung, boolean Feuer,boolean Absturz,boolean Grundstellung)
 {int i ;
  int PosTurmHeben;
  int PosTurmDrehen;
  PosTurmHeben  =   400 ;
  PosTurmDrehen =  1000 ;
  // Folgeradar: ausfahren
  SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
  SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenRMax  +8,SPEED_FolgeradarDrehenMax-10);
  Pause(0,4);
  TurmHeben.SetPosition(PosTurmHeben );
  TurmDrehen.SetPosition(PosTurmDrehen);

  // Folgeradar: Zielerfassung
  Pause(0,5);
  if(Erfassung)
  {for(i=1;i<=2;i++)
   {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
    Pause(0,3);
    SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +50,10);
    Pause(0,3);
   }
  }
  //Zielverfolgung
  if(Verfolgung)
  {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +30,10);
   SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenRMax  + 2,SPEED_FolgeradarDrehenMax);
   PosTurmDrehen += 90 ;
   TurmDrehen.SetPosition(PosTurmDrehen);
   Pause(0,5);
   for(i=1;i<500;i+=1)
   {PosTurmDrehen += 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
   }
  }
  //Zielbekämpfung
  if(Feuer)
  {SD21.SetPos(ADR_SoundKey,POS_SoundWaffeFeuer);
   for(i=1;i<300;i+=1)
   {int x = 0;
    PosTurmDrehen += 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    /*
    if(x == 0)        {TurmHeben.SetPosition(450);    }
    else if( x == 20) {TurmHeben.SetPosition(300);    }
    else if( x == 40) {TurmHeben.SetPosition(500);    }
    else if( x == 60) {TurmHeben.SetPosition(300);    }
    else if( x == 80) {TurmHeben.SetPosition(500);    }
    else if( x == 100) {TurmHeben.SetPosition(300);    }
    else if( x == 120) {TurmHeben.SetPosition(500);    }
    else if( x == 140) {TurmHeben.SetPosition(300);    }
    else if( x == 160) {TurmHeben.SetPosition(500);    }
    else if( x == 180) {TurmHeben.SetPosition(400);    }
    */
   }
   SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  }
  //Zielbekämpfung
  if(Absturz)
  {for(i=1;i<400;i+=1)
   {int x = 0;
    PosTurmDrehen += 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    PosTurmHeben -= 1 ;
    TurmHeben.SetPosition(PosTurmHeben);
   }
  }
  // Folgeradar Zurücksetzen
  if(Grundstellung)
  {TurmHeben.SetPosition(0);
   TurmDrehen.SetPosition(0);
   SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
   SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
   SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
   SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
   Pause(1);
  }
 }
 //_____________________________________________________________________________
 private static void ZielerfassungL2 (boolean Erfassung, boolean Verfolgung, boolean Feuer,boolean Absturz,boolean Grundstellung)
 {int i ;
  int PosTurmHeben;
  int PosTurmDrehen;
  PosTurmHeben  =   600 ;
  PosTurmDrehen =  3000 ;
  // Folgeradar: ausfahren
  SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
  SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenRMax  +8,SPEED_FolgeradarDrehenMax-10);
  Pause(0,4);
  TurmHeben.SetPosition(PosTurmHeben );
  TurmDrehen.SetPosition(PosTurmDrehen);

  // Folgeradar: Zielerfassung
  Pause(0,5);
  if(Erfassung)
  {for(i=1;i<=2;i++)
   {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +20,10);
    Pause(0,3);
    SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +50,10);
    Pause(0,3);
   }
  }
  //Zielverfolgung
  if(Verfolgung)
  {SD21.SetPos(ADR_FolgeradarHeben   ,POS_FolgeradarHebenCenter +30,10);
   SD21.SetPos(ADR_FolgeradarDrehen  ,POS_FolgeradarDrehenRMax  - 2,SPEED_FolgeradarDrehenMax);
   PosTurmDrehen -= 90 ;
   TurmDrehen.SetPosition(PosTurmDrehen);
   Pause(0,5);
   for(i=1;i<500;i+=1)
   {PosTurmDrehen -= 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
   }
  }
  //Zielbekämpfung
  if(Feuer)
  {SD21.SetPos(ADR_SoundKey,POS_SoundWaffeFeuer);
   for(i=1;i<600;i+=1)
   {int x = 0;
    PosTurmDrehen -= 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    /*
    if(x == 0)        {TurmHeben.SetPosition(450);    }
    else if( x == 20) {TurmHeben.SetPosition(300);    }
    else if( x == 40) {TurmHeben.SetPosition(500);    }
    else if( x == 60) {TurmHeben.SetPosition(300);    }
    else if( x == 80) {TurmHeben.SetPosition(500);    }
    else if( x == 100) {TurmHeben.SetPosition(300);    }
    else if( x == 120) {TurmHeben.SetPosition(500);    }
    else if( x == 140) {TurmHeben.SetPosition(300);    }
    else if( x == 160) {TurmHeben.SetPosition(500);    }
    else if( x == 180) {TurmHeben.SetPosition(400);    }
    */
   }
   SD21.SetPos(ADR_SoundKey,POS_SoundKeyS0);
  }
  //Zielbekämpfung
  if(Absturz)
  {for(i=1;i<400;i+=1)
   {int x = 0;
    PosTurmDrehen -= 1 ;
    TurmDrehen.SetPosition(PosTurmDrehen);
    PosTurmHeben -= 1 ;
    TurmHeben.SetPosition(PosTurmHeben);
   }
  }
  // Folgeradar Zurücksetzen
  if(Grundstellung)
  {TurmHeben.SetPosition(0);
   TurmDrehen.SetPosition(0);
   SD21.SetPos(ADR_PeriskopLinksDrehen ,POS_PeriskopLinksCenter   ,0);
   SD21.SetPos(ADR_PeriskopRechtsDrehen,POS_PeriskopRechtsCenter  ,0);
   SD21.SetPos(ADR_FolgeradarDrehen    ,POS_FolgeradarDrehenCenter,SPEED_FolgeradarDrehenMax);
   SD21.SetPos(ADR_FolgeradarHeben     ,POS_FolgeradarHebenCenter ,0);
   Pause(1);
  }
 }
 //_____________________________________________________________________________
 // Programm-Startpunkt
 public static void main()
 {boolean res ;
  int val ;
  /*********SharedI2CBus initialisieren************/
  //I2CBus-Treiber 'SharedI2CBus' erstellen
  SharedI2CBus = new I2C(CPU.pin0, CPU.pin1);
  /*********SD21 initialisieren************/
  //SD21-Treiber 'SD21' erstellen
  SD21         = new SD21(SharedI2CBus);
  //alle SD21-Servokanäle in Fail-Save-Positionen setzen
  SD21_FailSave();
  /*********TMC222 initialisieren************/
  //Schrittmotor-Treiber 'TurmDehen' erstellen
  TurmDrehen = new TMC222(SharedI2CBus,I2CADR_TMC222_TurmDrehen);
  //Schrittmotor-Parameter 'TurmDehen' setzen
  TurmDrehen.ResetToDefault();
  TurmDrehen.GetFullStatus1();
  TurmDrehen.Irun     = 15 ;
  TurmDrehen.Ihold    =  8 ;
  TurmDrehen.Vmax     =  4 ;
  TurmDrehen.Vmin     =  1 ;
  TurmDrehen.Acc      =  1 ;
  TurmDrehen.StepMode =  2 ;
  TurmDrehen.SetMotorParam();
  //Schrittmotor-Treiber 'TurmHeben' erstellen
  TurmHeben  = new TMC222(SharedI2CBus,I2CADR_TMC222_TurmHeben );
  //Schrittmotor-Parameter 'TurmHeben' setzen
  TurmHeben.ResetToDefault();
  TurmHeben.GetFullStatus1();
  TurmHeben.Irun     = 15 ;
  TurmHeben.Ihold    =  8 ;
  TurmHeben.Vmax     =  1 ;
  TurmHeben.Vmin     =  1 ;
  TurmHeben.Acc      =  1 ;
  TurmHeben.StepMode =  2 ;
  TurmHeben.SetMotorParam();
  //Referenzposition ermitteln
  //TurmHeben.RunInit(4,1,6000,50);
  //Referenzposition ermitteln
  //TurmDrehen.RunInit(4,1,6000,50);


  //TurmDrehen.PrintFullStatus1();
  //TurmHeben.PrintFullStatus1();

  /***********Empfänger initialisieren *************
  Empfänger Empfänger = new Empfänger(SD21);
  **************************************************/
  Grundstellung(1);
  Sound(POS_SoundMotorAnAus);

/*
  val=1;
  while(val<100)
  {
   LuftraumUberwachung(5);
   ZielerfassungR2(false,true,false,false,true);
   ZielerfassungR2(true,true,true,true,true);
  }
*/


  while(true)
  {Grundstellung(10);
   LuftraumUberwachung(10);
   ZielerfassungR1(false,false,false,false,false);
   ZielerfassungL1(true,true,true,true,true);
   LuftraumUberwachung(20);
   ZielerfassungR1(true,true,true,true,true);
   LuftraumUberwachung(60);
   Sound(POS_SoundMotorZwischengas);
   LuftraumUberwachung(120);
   Sound(POS_SoundMotorZwischengas);
   LuftraumUberwachung(30);
   ZielerfassungL1(false,false,false,false,true);
   LuftraumUberwachung(30);
   ZielerfassungR2(true,true,true,true,false);
   ZielerfassungR2(true,true,true,true,true);
   LuftraumUberwachung(5);
   ZielerfassungL1(false,true,false,false,true);
   LuftraumUberwachung(3);
   ZielerfassungL1(true,true,true,true,true);
   LuftraumUberwachung(50);
   Sound(POS_SoundMotorZwischengas);
   LuftraumUberwachung(120);
   Grundstellung(10);
   Sound(POS_SoundMotorAnAus);
   Zigarettenpause(10);
   Sound(POS_SoundMotorZwischengas);
   Zigarettenpause(60);
   Sound(POS_SoundMotorZwischengas);
   Zigarettenpause(180);
   LuftraumUberwachung(60);
   Sound(POS_SoundMotorZwischengas);
  }
 }

}

/*____________________________________________________________________________*/
class Empfänger
{public int Line1 = 0 ;
 public int Line2 = 0 ;
 public int Line3 = 0 ;
 public int Line4 = 0 ;
 public int Line5 = 0 ;
 public int Line6 = 0 ;
 public int Line7 = 0 ;
 public int Line8 = 0 ;
 public int Line9 = 0 ;

 private int AutoCenter1 = 0 ;
 private int AutoCenter2 = 0 ;
 private int AutoCenter3 = 0 ;
 private int AutoCenter4 = 0 ;
 private int AutoCenter5 = 0 ;
 private int AutoCenter6 = 0 ;
 private int AutoCenter7 = 0 ;
 private int AutoCenter8 = 0 ;
 private int AutoCenter9 = 0 ;
 private SD21 Servo ;


 /**********************************************************/
 /** Create object                                        **/
 /**                                                      **/
 /** @param bus I2C bus object                            **/
 /**********************************************************/
 public Empfänger (SD21 Servo)
 {this.Servo = Servo ;
 }
 /**********************************************************/
 public void Init ()
 {int i ;
  // warten, bis Empfänger daten bereitstellt
  while(CPU.pulseIn(2400,CPU.pin2,true)<=0) ;
  // ersten 10 Werte einlesen
  for(i=0 ; i<10 ; ++i)
  {AutoCenter1 += CPU.pulseIn(2400,CPU.pin2,true);
   AutoCenter2 += CPU.rcTime(400,CPU.pin4,false);
   AutoCenter3 += CPU.rcTime(400,CPU.pin6,false);
   AutoCenter4 += CPU.rcTime(400,CPU.pin8,false);
  }
  // Mittelwert bilden
  AutoCenter1 /= 10 ;
  AutoCenter2 /= 10 ;
  AutoCenter3 /= 10 ;
  AutoCenter4 /= 10 ;
 }
 /**********************************************************/
 public void Scan ()
 {
   Line1 += CPU.pulseIn(2400,CPU.pin2,true)- AutoCenter1 ;
   Line2 += CPU.rcTime(400,CPU.pin4,false) - AutoCenter2 ;
   Line3 += CPU.rcTime(400,CPU.pin6,false) - AutoCenter3 ;
   Line4 += CPU.rcTime(400,CPU.pin8,false) - AutoCenter4 ;
   Line1 /= 2;
   Line2 /= 2;
   Line3 /= 2;
   Line4 /= 2;
/*****
   if(val1 > 0)
   {val2 = CPU.rcTime(400,CPU.pin4,false);
    val3 = CPU.rcTime(400,CPU.pin6,false);
    val4 = CPU.rcTime(400,CPU.pin8,false);
   }
   else
   {val2 = 0;
    val3 = 0;
    val4 = 0;
   }
*********/


 }
}


/*_EOF________________________________________________________________________*/