IMU mit Matlab/Simulink: Unterschied zwischen den Versionen

Aktuelle Version vom 5. Juli 2018, 22:59 Uhr

→ zurück zum Hauptartikel: SigSys SoSe2018

Autoren: Sebastian Dany
Betreuer: Prof. Dr. Ulrich Schneider
Sensor: dIMU

Einleitung

Für die Semesterbegleitende Prüfung der Studierenden des Masterstudienganges "Business and Systems Engineering" im Teilmodul "Signalverarbeitende Systeme" im Modul "Systementwurf" wurden verschiedene Sensoren mit verschiedenen Komplexitätsgraden ausgegeben. Die Aufgabe bestand darin, diese Sensoren zu verstehen, zu beschreiben, auszulesen und zu kalibrieren.

Primärsensor

Als Primärsensor dient der dIMU-Sensor der Firma Dexter Industries, welcher mittels den zwei eingebauten Sensoren die Rotationsbeschleunigung und die Beschleunigung in jede Raumrichtung ausgeben soll. Die verbauten Sensoren werden mit unterschiedlichen Adressen angesprochen und könn en mittels einem für LEGO modifizierten RJ11-Verbindungskabel mit einem NXT Baustein, oder einem EV3 Baustein verbunden und ausgelesen werden. Alternativ ist es auch mit einem Arduino möglich, den Sensor auszulesen. Dazu muss der Baustein oder der Arduino (Master) Daten über I2C oder SPI vom Sensor (Slave) anfordern. Dieser liefert dann nach Abschluss der Anfrage des Masters die Daten. Beide Sensoren bauen auf demselben physikalischen Prinzip auf, nämlich der Massenträgheit und der Veränderung der Kapazität bei Abstandsänderung der Kondensatoroberflächen. Der Temperatursensor, der in beiden Sensormodulen verbaut ist, ist ein Silizium-Messwiderstand, welcher im Bereich von -40 °C bis +85 °C einsetzbar ist. Das Accelerometer (Adresse: 0x1D) sollte laut Datenblatt nicht mit über 3,6 Volt versorgt werden. Die auf der Platine gesetzten Widerstände schützen den Sensor vor den normalerweise bei dem Kabel anliegenden 4,3 V mit 20 mA. Der Gyrosensor und das Accelerometer geben je nach Typ, unterschiedliche Bitlängen an Daten aus: Beim Gyrosensor (Adresse: 0x69) liegen die Beschleunigungsausgaben als Rotationsbeschleunigung um die drei Raumachsen bei 16-Bit-Werten und bei dem eingebauten Temperatursensor bei maximal 8-Bit-Werten. Alle Werte werden in das Register geschrieben. Das Gerät kann in der Sensitivität auf +-250/+-500/+-2000 dps (degres per second) gesetzt werden, die Auflösung wird damit feiner. Bei der Initialisierung kalibriert der Gyrosensor Offset und Gain nach Datenblatt selbstständig und ist in der Lage die Daten mit verschiedenen Filtern zu verarbeiten. Eine Kalibrierung des Gyrosensors wurde dennoch nicht festgestellt. Datei:DatenblattAcc.pdf Datei:DatenblattGyr.pdf

Das Accelerometer misst die Beschleunigung entlang der drei Raumachsen und speichert diese als 16-Bit Wert im Register ab. Dabei wird angegeben, dass die Ausgabewerte in der Einheit mG (ein tausendstel der Erdbeschleunigung) abgespeichert werden. Der Temperatursensor beschreibt das Register ebenfalls mit 8-Bit. Beide Temperatursensoren arbeiten mit einer Frequenz von 1 Hz und sind mit einem Offset behaftet. Die Daten entsprechen dem Temperaturunterschied zwischen Messzeitpunkt und initialisierung in °C. Beide Sensoren lesen die Messwerte nacheinander aus, damit sind sie minimal Zeitversetzt. Der Gyrosensor meldet eine Wertänderung durch Änderung des Registereintrages (Status_REG, Bit 7) an, sodass nicht immer alle Register ausgelesen werden müssen.

Auf dem Arduino werden die Daten durch einen rekursiven Mittelwertfilter geglättet und an die Ausgabe übergeben!

Analog-Digital-Umsetzer (DAU)

Der in den Sensoren verwendete DAU lässt sich im Datenblatt nicht ermitteln. Aufgrund von hohen Aktualisierungsraten und geringer Bitlänge der Daten (maximal 16-Bit beim Accelerometer) ist die Realisierung eines Parallelumsetzers wahrscheinlich. Die Kosten für diesen Umsetzer halten sich im Rahmen, da maximal 8 Bitstufen in zwei Stufen, nacheinander ausgelesen werden. Der Sensor benötigt daher 30 Komparatoren für einen zweistufigen Acht-Bit-Umsetzer, wohingegen der Flash-Umsetzer 255 benötigt. Dabei werden die Daten nacheinander in den Umsetzer geladen und nacheinander in die Register geschrieben. Der Pipeline-Umsetzer bietet eine günstige Alternative zu Flash-Umsetzern. Eine Realisierung mit einem Flash-Umsetzer hätte dagegen den Vorteil, dass die Daten mit einer schnelleren Taktung gewandelt werden könnten.

Bussystem

Zwischen dem Sensor und dem Mikrokontroller wird per I2C (Inter-Integrated Circuit) kommuniziert. Es handelt sich um einen seriellen Datenbus, bei dem nur 2 Datenleitungen benötigt werden und bei dem nur ein Gerät im Netz die Position des Masters übernimmt. Die zwei Datenleitungen sind SCL und SDA. SCL wird vom Master ausgegeben und gibt die Taktung der Bits vor. SDA dient der reinen Datenübertragung. Beim Arduino Uno werden SDA und A4 für die Datenübertragung, sowie SCL und A5 für die Taktung genutzt. Diese sind standardmäßig in der ‚Wire.h‘-Bibliothek reserviert. Aus einer Information werden drei Pakete geschnürt und die Kommunikation läuft wie folgt ab:

Start-byte wird gesendet.
Adresse des Angesprochenen Slaves wird vom Master gesendet (Paket).
Empfangsbestätigung (1Bit). Bei Nein, geht es weiter bei Punkt 8.
Schreiben/Lesen (1Bit).
Registeradresse zum Auslesen/Beschreiben wird beim Slave angemeldet (Paket).
Soll mehr angefragt werden? Bei Nein, geht es weiter bei Punkt 8.
Daten zum Auslesen/Beschreiben werden vom Slave/Master gesendet (Paket). Wenn mehr angefragt werden soll, geht es weiter bei Punkt 3
Empfangsbestätigung (1Bit). Bei Nein, geht es weiter bei Punkt 8.
Beende die Kommunikation mit dem End-byte.

[1]

Herstellen der Kommunikation mit dem Arduino UNO

Pinbelegung für Sensoren
Pin	Kabelfarbe	Steckplatz am Arduino
Pin1	Weiß	wird nicht verbunden
Pin2	Schwarz	GND
Pin3	Rot	GND
Pin4	Grün	3V3 (3,3 V)
Pin5	Gelb	SCL (per 10k Pullup auf 3V3)
Pin6	Blau	SDA (per 10k Pullup auf 3V3)

Für die Datenausgabe wird ein TFT-Shield-Display genutzt, welches per Dupont-kabel mit dem Arduino verbunden wird. Der Reset-Pin des Shields wird an Pin 12 verlegt, da er ansonsten auf dem ebenfalls für I²C-Kommunikation genutzten Pin A4 liegt. Wäre der Reset-Pin mit Pin A4 verbunden, gäbe das Display nur weißes Licht von sich, da es durchgehend zurückgesetzt wird. Ansonsten bleibt die Pin-belegung des Shields gleich.

Video Url: [2]

//dIMU Auslesen 
//Entwickelt von Sebastian Dany


#include <Wire.h>
#include <Elegoo_GFX.h>    // Core graphics library Display
#include <Elegoo_TFTLCD.h> // Hardware-specific library Display
#include <TouchScreen.h>

// Display initiierung
#define LCD_CS A3 // Chip Select goes to Analog 3
#define LCD_CD A2 // Command/Data goes to Analog 2
#define LCD_WR A1 // LCD Write goes to Analog 1
#define LCD_RD A0 // LCD Read goes to Analog 0
#define YP A3  // must be an analog pin, use "An" notation!
#define XM A2  // must be an analog pin, use "An" notation!
#define YM 9   // can be a digital pin
#define XP 8   // can be a digital pin
#define TS_MINX 120
#define TS_MAXX 900
//240x320
#define TS_MINY 70
#define TS_MAXY 920
#define LCD_CS A3
#define LCD_CD A2
#define LCD_WR A1
#define LCD_RD A0

#define LCD_RESET 12
// Can alternately just connect to Arduino's reset pin
#define BOXSIZE 40
#define PENRADIUS 3

// Assign human-readable names to some common 16-bit color values:
#define  BLACK   0x0000
#define BLUE    0x001F
#define RED     0xF800
#define GREEN   0x07E0
#define CYAN    0x07FF
#define MAGENTA 0xF81F
#define YELLOW  0xFFE0
#define WHITE   0xFFFF
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
Elegoo_TFTLCD tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);
int SwitchCase=0;
int OldCase=0;
int MessagescreenX=TS_MAXX-TS_MINX;
int MessagescreenY=TS_MAXY-TS_MINY;
int Aussenbegrenzung=3;
int AuswahlboxX=90;
int AuswahlboxY=40;
int identifier=tft.readID();
int X_Label_Daten=5;
int X_Ausgabe_Daten=65;
int Y_Schrittweite_Daten=18;
//Initiierung der Adressen des Sensors
int Einstellung_abgeschlossen=0;

int Adresse_Gyr1; //210 Gyr 0xD2
int Adresse_Acc1; //50  Acc 0x3A
int Adresse_Acc=0x1D; //Nach Datenblatt/2
int Adresse_Gyr=0x69; //Nach Datenblatt/2
long Time_start;
long MeasurementTime;
int MeasureCtr=0;
int Abgeschlossen=0;
int Multi=2^8; //Offset von OUT_X_H

//OUTPUT 
      int XL;
      int XH;
      int YL;
      int YH;
      int ZL;
      int ZH;
      int Temp;
      //////////////////////////////////////////////////////////////////Gyrosensor Register
      
      int GYR_WHO_AM_I  =0x0F;
      int CTRL_REG1     =0x20; // 0F  //Stellt den Operationsmodus ein. *Immer wieder nach 5 ms machen.
      int CTRL_REG2     =0x21; // 0|0|HPM1|HPM0|HPCF3|HPCF2|HPCF1|HPCF0 --High Pass Filter-- Default: 0|0|0|0|0|0|0|0|0 100Hz cut-off
      int CTRL_REG3     =0x22; // Schreib ich nicht hin --Interrupts-- Default: 0x80  1|0|0|0|0|0|0|0|0
      int CTRL_REG4     =0x23; // Default: 0x00  0|0|0|0|0|0|0|0|0
      int CTRL_REG5     =0x24; //--Filter--Default: 0x00  0|0|0|0|0|0|0|0|0
      int REFERENCE     =0x25; //
      int OUT_Temp      =0x26;  //7-Bit länge
      int STATUS_REG    =0x27;
      int OUT_X_L       =0x28;
      int OUT_X_H       =0x29;
      int OUT_Y_L       =0x2A;
      int OUT_Y_H       =0x2B;
      int OUT_Z_L       =0x2C;
      int OUT_Z_H       =0x2D;
      int FIFO_CTRL_REG =0x2E;
      int FIFO_SRC_REG  =0x2F;
      int INT1_CFG      =0x30;
      int INT1_SRC      =0x31;
      int INT1_TSH_XH   =0x32;
      int INT1_TSH_XL   =0x33;
      int INT1_TSH_YH   =0x34;
      int INT1_TSH_YL   =0x35;
      int INT1_TSH_ZH   =0x36;
      int INT1_TSH_ZL   =0x37;
      int INT1_DURATION =0x38;
 


      //Startup Sequences GYRO
        //Selective Axes
      int FOR_CTRL_REG1= 0x0F;
      int FOR_CTRL_REG3= 0x80;
      int FOR_INT1_THS_XH=0x2C;
      int FOR_INT1_DUR_XL=0xA4;


      //////////////////////////////////////////////////////////////////Accelerometer Register
      
      int XOUTL         =0x00; //10 bit value X LSB
      int XOUTH         =0x01; //10 bit value X MSB
      int YOUTL         =0x02; //10 bit value Y LSB
      int YOUTH         =0x03; //10 bit value Y MSB
      int ZOUTL         =0x04; //10 bit value Z LSB
      int ZOUTH         =0x05; //10 bit value Z MSB
      int XOUT8         =0x06; // 8 bit value X
      int YOUT8         =0x07; // 8 bit value Y
      int ZOUT8         =0x08; // 8 bit value Z
      int STATUS        =0x09; // Status Registers
      int DETSRC        =0x0A;
      int TOUT          =0x0B; //Temperatur OUT
      int ACC_WHO_AM_I  =0x0F;
      int XOPFFL        =0x10;
      int XOPFFH        =0x11;
      int YOPFFL        =0x12;
      int YOPFFH        =0x13;
      int ZOPFFL        =0x14;
      int ZOPFFH        =0x15;
      int MCTL          =0x16;
      int CONTROL1      =0x18;
      int LDTH          =0x1A;
      int PDTH          =0x1B;
      
      

      int For_MCTL      = B00000101; //2g Measurement Mode
      int For_CONTROL1  = B00000000; //Disable Axes
      int For_LDTH      = B00000000; //Disable Axes

     // int g_mode = 0x02;          // 0x00 = +/- 2g  // 0x01 = +/- 4g  // 0x02 = +/- 8g

////////////////////////////////////////////////////////////////Werterotation Variablen zur Berechnung
int AccXn0=0;
int AccYn0=0;
int AccZn0=0;
int AccTn0=0;

int AcXXn0=0;
int AcYYn0=0;
int AcZZn0=0;


//////////////Rechenvariabeln
//Änderungen
int AccdX=0;
int AccdY=0;
int AccdZ=0;
int AccdT=0;

//Mittelwerte
int AccMWX=0;
int AccMWY=0;
int AccMWZ=0;
int AccMWT=0;
//Ausgabe
int MW_Beschl_X;
int MW_Beschl_Y;
int MW_Beschl_Z;
int Rotation_um_X;
int Rotation_um_Y;
int Rotation_um_Z;
int MW_Temp;
//Referenz
int Ref_XX=0;
int Ref_YY=0;
int Ref_ZZ=0;
int Korr_XX;
int Korr_YY;
int Korr_ZZ;
//Mittelwertbestimmung
int MAccXn0=0;
int MAccYn0=0;
int MAccZn0=0;
int MAccTn0=0;
int MAcXXn0=0;
int MAcYYn0=0;
int MAcZZn0=0;  
int AcXXn6=0;
int AcYYn6=0;
int AcZZn6=0;
int AcXXn5=0;
int AcYYn5=0;
int AcZZn5=0;
int AcXXn4=0;
int AcYYn4=0;
int AcZZn4=0;
int AcXXn3=0;
int AcYYn3=0;
int AcZZn3=0;
int AcXXn2=0;
int AcYYn2=0;
int AcZZn2=0;
int AcXXn1=0;
int AcYYn1=0;
int AcZZn1=0;
int AccXn6=0;
int AccYn6=0;
int AccZn6=0;
int AccTn6=0;
int AccXn5=0;
int AccYn5=0;
int AccZn5=0;
int AccTn5=0;
int AccXn4=0;
int AccYn4=0;
int AccZn4=0;
int AccTn4=0;
int AccXn3=0;
int AccYn3=0;
int AccZn3=0;
int AccTn3=0;
int AccXn2=0;
int AccYn2=0;
int AccZn2=0;
int AccTn2=0;
int AccXn1=0;
int AccYn1=0;
int AccZn1=0;
int AccTn1=0; 

int AccMWXX=0;
int AccMWYY=0;
int AccMWZZ=0;  

int GyrXn0;
int GyrYn0;
int GyrZn0;
int GyrTn0;
////////////////////////////////////////////////////////////////////////////////////////////////////////////Programme
void DrawError(int Text){
  tft.fillRect(0,0,MessagescreenX,MessagescreenY,BLACK);
  tft.fillRect(Aussenbegrenzung, Aussenbegrenzung,MessagescreenX-2*Aussenbegrenzung,MessagescreenY-2*Aussenbegrenzung,BLUE);
  tft.setCursor(0,0);

 // tft.fillRect////////////////////////////////////////////////////////working on ok and Cancel
}

  
  void WriteRegister(int Register, int Wert)
{     

  Wire.write(Register);
  Wire.write(Wert);
  }
  
int ReadRegister(int Adresse, int Register, int Byte)
{
//  int Ausgabe;
  Wire.beginTransmission(Adresse);
  Wire.write(Register);
  Wire.endTransmission(Adresse);
//  delay(1);
  Wire.requestFrom(Adresse, Byte);
  while(Wire.available()){
    return Wire.read();
  }
//  return Ausgabe ;
}

int WakeUpGyr()
{
  Wire.beginTransmission(Adresse_Gyr);
  WriteRegister(CTRL_REG1, 0x01);
  Wire.write(STATUS_REG);
  Serial.println(Wire.endTransmission());
  Serial.println("'Wake up'");
}
     ///////////////////////Initialisierungsfunktion
void Initialisiere(){

  tft.fillRect(0,0,MessagescreenY, MessagescreenX, BLACK);
  tft.setCursor(0, 0);
  tft.setTextColor(WHITE);    tft.setTextSize(2);
   tft.println("Initialisiere...");

    WakeUpGyr();
    while(Abgeschlossen==0)
    {      
      byte error, address;
      int nDevices=0;
      tft.setTextSize(1);
      Serial.println("...");
      delay(1000);           // wait 1 second
      nDevices = 0;
      for(address = 1; address < 127; address++ )
        {
        // The i2c_scanner uses the return value of
        // the Write.endTransmisstion to see if
        // a device did acknowledge to the address.
        Wire.beginTransmission(address);
        error = Wire.endTransmission();
     
        if (error == 0)
           {
             Serial.print("I2C device found at address 0x");
              if (address<16)
               Serial.print("0");
             Serial.print(address,HEX);
             Serial.println(" !");
             if(nDevices==0)
             Adresse_Acc1=(address);
             if(nDevices==1)
             Adresse_Gyr1=(address);
                  
           nDevices++;
            }
           else if (error==4)
            {
            Serial.print("Unknown error at address 0x");
            if (address<16)
              Serial.print("0");
            Serial.println(address,HEX);
            }    
            
          }
      if (nDevices == 0){
        tft.println("Keine Adressen gefunden!");
        }
      else 
        Abgeschlossen=1;
        tft.println("Adressen der Sensoren gefunden!");
      
        
    }                     //Adressenprüfung abgeschlossen
  

Serial.println("");
Serial.println("Die Adresse im Datenblatt ist verdoppelt, daher wird sie halbiert!");
tft.print("Adresse des Accelerometers: ");
//Serial.print("0x");
//if (Adresse_Acc<16)
// Serial.print("0");
tft.println(Adresse_Acc1,HEX);
tft.print("Soll Adresse              : ");
tft.println(Adresse_Acc,HEX);
//Serial.println(A,BIN);
if(Adresse_Acc1==Adresse_Acc)
tft.println("-> Ist identisch mit den hinterlegten Adressen!");

else
  {
  tft.println("-> Ist nicht identisch mit den hinterlegten Adressen");
  Abgeschlossen=0;
  }

  
tft.print("Adresse des Gyroskops     : ");
//Serial.print("0x");
//if (Adresse_Gyr<16)
// Serial.print("0");
tft.println(Adresse_Gyr1,HEX);
tft.print("Soll Adresse              : ");
tft.println(Adresse_Gyr,HEX);
if(Adresse_Gyr1==Adresse_Gyr)
tft.println("-> Ist identisch mit den hinterlegten Adressen!");
else
  {
  tft.println("-> Ist nicht identisch mit den hinterlegten Adressen");
  Abgeschlossen=0;
  }
Adresse_Acc=Adresse_Acc1;
Adresse_Gyr=Adresse_Gyr1;
//return Abgeschlossen;
}

int Einstellen(){
  
  int Abgeschlossen=0;
  int GyrAbgeschlossen=0;
  int AccAbgeschlossen=0;
    Serial.println("");
    Serial.println("   *************************************************************");
    Serial.println("   ****************** Einstellung der Sensoren *****************");
    Serial.println("   *************************************************************");
    Serial.println("");
    
    ////////////////////////////////////////////Gyrosensor
        Wire.beginTransmission(Adresse_Gyr);
        WriteRegister(CTRL_REG1,    0x0F);//0x0F
        WriteRegister(CTRL_REG2,    0x00);
        WriteRegister(CTRL_REG3,    0x80);//0x80
        WriteRegister(CTRL_REG4,    0x00);
        WriteRegister(CTRL_REG5,    0x00);//0x05
        WriteRegister(REFERENCE,    0x00);//127
        WriteRegister(INT1_TSH_XH,  0x2C);//2C
        WriteRegister(INT1_TSH_XL,  0xA4);//A4
        WriteRegister(INT1_TSH_YH,  0x2C);//0x2C
        WriteRegister(INT1_TSH_YL,  0xA4);//0xA4
        WriteRegister(INT1_TSH_ZH,  0x2C);//0x2C
        WriteRegister(INT1_TSH_ZL,  0xA4);//0xA4
        WriteRegister(INT1_DURATION,0x00);//0x01
        WriteRegister(INT1_CFG,     0x02);//0x65
        //Wire.endTransmission(Adresse_Gyr);

        GyrAbgeschlossen=Wire.endTransmission(Adresse_Gyr);
        
    ////////////////////////////////////////////Accelerometer
        Wire.beginTransmission(Adresse_Acc);
        WriteRegister(MCTL, For_MCTL);
        WriteRegister(CONTROL1, For_CONTROL1);
        WriteRegister(LDTH, For_LDTH);
        //Wire.endTransmission(Adresse_Acc);
        Serial.print("    ");
        AccAbgeschlossen=Wire.endTransmission(Adresse_Acc);
        Abgeschlossen=GyrAbgeschlossen+AccAbgeschlossen;
    if (Abgeschlossen==2)
    return 1;

    else if (Abgeschlossen==0)
    return 0;

    else
    {     
      if (AccAbgeschlossen==1) DrawError("Accelerometer kann nicht Kalibriert werden!");  ///evtl return value?
      else DrawError("Gyroskop kann nicht Kalibriert werden!");
    }
}

void StarteMessung(int Time_start)   //Nimm Werte auf
{
          MeasurementTime=millis()-Time_start;
          GyrXn0=ReadRegister(Adresse_Gyr, OUT_X_L, 8)+ReadRegister(Adresse_Gyr, OUT_X_H, 8)*Multi;//GyrXn0=ReadRegister(Adresse_Gyr, OUT_X_L, 8)+ReadRegister(Adresse_Gyr, OUT_X_H, 8)*Multi;
          GyrYn0=ReadRegister(Adresse_Gyr, OUT_Y_L, 8)+ReadRegister(Adresse_Gyr, OUT_Y_H, 8)*Multi;
          GyrZn0=ReadRegister(Adresse_Gyr, OUT_Z_L, 8)+ReadRegister(Adresse_Gyr, OUT_Z_H, 8)*Multi;
          GyrTn0=ReadRegister(Adresse_Gyr, Temp, 8);

          AccXn0=ReadRegister(Adresse_Acc, XOUTL, 8)+ReadRegister(Adresse_Acc, XOUTH, 8)*Multi;
          AccYn0=ReadRegister(Adresse_Acc, YOUTL, 8)+ReadRegister(Adresse_Acc, YOUTH, 8)*Multi;
          AccZn0=ReadRegister(Adresse_Acc, ZOUTL, 8)+ReadRegister(Adresse_Acc, ZOUTH, 8)*Multi;
          AccTn0=ReadRegister(Adresse_Acc, Temp, 8);

          AcXXn0=ReadRegister(Adresse_Acc, XOUT8, 8);
          AcYYn0=ReadRegister(Adresse_Acc, YOUT8, 8);
          AcZZn0=ReadRegister(Adresse_Acc, ZOUT8, 8);

//tft.println(AccXn0);//Test, ob Daten geliefert werden
//tft.println(AccYn0);
//tft.println(AccZn0);

}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////A
void setup()
{  

  Wire.begin();
   Serial.begin(9600);
  while (!Serial);
  Serial.println("");
  Serial.println("dIMU Scanner by Sebastian Dany");
  tft.begin(identifier);        
  tft.setRotation(1);
}
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////B
void loop()
{   

    //////////////////////////////////////////////Display ausgabe
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  TSPoint p = ts.getPoint();
  digitalWrite(13, LOW);
  pinMode(XM, OUTPUT);
  pinMode(YP, OUTPUT);
    if(SwitchCase!= 0){
        if (p.y < BOXSIZE) {  //Wenn sich die Auswahl des Modus verändert, verändert sich der Switch Case und die graphische Eingabe
       OldCase = SwitchCase;

         if (p.x < BOXSIZE) { 
           SwitchCase = 1; 
           tft.drawRect(0, 0, BOXSIZE, BOXSIZE, WHITE);
         } 
         else if (p.x < BOXSIZE*2) {
           SwitchCase = 2;
           tft.drawRect(BOXSIZE, 0, BOXSIZE, BOXSIZE, WHITE);
         } 
         else if (p.x < BOXSIZE*3) {
           SwitchCase = 3;
           tft.drawRect(BOXSIZE*2, 0, BOXSIZE, BOXSIZE, WHITE);
         } 
         else if (p.x < BOXSIZE*4) {
           SwitchCase = 4;
           tft.drawRect(BOXSIZE*3, 0, BOXSIZE, BOXSIZE, WHITE);
         } 
         else if (p.x < BOXSIZE*5) {
           SwitchCase = 5;
           tft.drawRect(BOXSIZE*4, 0, BOXSIZE, BOXSIZE, WHITE);
         } 
         else if (p.x < BOXSIZE*6) {
           SwitchCase = 6;
           tft.drawRect(BOXSIZE*5, 0, BOXSIZE, BOXSIZE, WHITE);
         }
  
         if (OldCase != SwitchCase) {
            if (OldCase == 1) tft.fillRect(0, 0, BOXSIZE, BOXSIZE, RED);
            if (OldCase == 2) tft.fillRect(BOXSIZE, 0, BOXSIZE, BOXSIZE, YELLOW);
            if (OldCase == 3) tft.fillRect(BOXSIZE*2, 0, BOXSIZE, BOXSIZE, GREEN);
            if (OldCase == 4) tft.fillRect(BOXSIZE*3, 0, BOXSIZE, BOXSIZE, CYAN);
            if (OldCase == 5) tft.fillRect(BOXSIZE*4, 0, BOXSIZE, BOXSIZE, BLUE);
            if (OldCase == 6) tft.fillRect(BOXSIZE*5, 0, BOXSIZE, BOXSIZE, MAGENTA);
            SwitchCase=OldCase;
         }
                  
       }
    }
    
    switch (SwitchCase){
      tft.begin(identifier);
      
      case 0:  //Initiierung
      {
        Initialisiere();
        delay(2000);
        tft.setRotation(1);
        tft.fillRect(0,0,400,300,BLUE);
        SwitchCase =1;
      }
      break;
      
      case 1:  //Voreinstellungen des Displays für die Messung
      { tft.setTextSize(2);
        int Ctr=0;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("X   :");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("Y   :");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr); 
        tft.println("Z   :");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("AccX:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("AccY:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("AccZ:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("ATem:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("GyrX:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("GyrY:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("GyrZ:");
        Ctr++;
        tft.setCursor(X_Label_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println("GTem:");
      SwitchCase=2;
      }      
      break;

      case 2:  //Messungen darstellen Zahlen
      { 
        Time_start=millis();
        MeasurementTime, AccXn0, AccYn0, AccZn0, AccTn0, AcXXn0, AcYYn0, AcZZn0=0,0,0,0,0,0,0,0;//Reset
        for (int MessungCtr=1;MessungCtr++;){
        
        
        StarteMessung(Time_start);
        int ModMess=MessungCtr % 5;
                    
              if (ModMess==1){     //Aufgrund von vielen Schreibzugriffen, werden nur die ältesten Daten überschrieben.
            
                  MAccXn0=AccXn0; //Werte überschreiben
                  MAccYn0=AccYn0;
                  MAccZn0=AccZn0;
                  MAccTn0=AccTn0;
                  MAcXXn0=AcXXn0;
                  MAcYYn0=AcYYn0;
                  MAcZZn0=AcZZn0;
                  }
              else {
              if (ModMess==2){
                  AccXn1=AccXn0; //Werte überschreiben
                  AccYn1=AccYn0;
                  AccZn1=AccZn0;
                  AccTn1=AccTn0;
                  AcXXn1=AcXXn0;
                  AcYYn1=AcYYn0;
                  AcZZn1=AcZZn0;
                  }
                  else{
                  if (ModMess==3)
                    {
                    AccXn2=AccXn0; //Werte überschreiben
                    AccYn2=AccYn0;
                    AccZn2=AccZn0;
                    AccTn2=AccTn0;
                    AcXXn2=AcXXn0;
                    AcYYn2=AcYYn0;
                    AcZZn2=AcZZn0;
                    }
                    else{
                    if (ModMess==4)
                      {
                      AccXn3=AccXn0; //Werte überschreiben
                      AccYn3=AccYn0;
                      AccZn3=AccZn0;
                      AccTn3=AccTn0;
                      AcXXn3=AcXXn0;
                      AcYYn3=AcYYn0;
                      AcZZn3=AcZZn0;
                      }
                      else{
                      if (ModMess==5){ 
                        AccXn4=AccXn0; //Werte überschreiben
                        AccYn4=AccYn0;
                        AccZn4=AccZn0;
                        AccTn4=AccTn0;
                        AcXXn4=AcXXn0;
                        AcYYn4=AcYYn0;
                        AcZZn4=AcZZn0;
                        }
                        else{    //==0
                        
                          AccXn3=AccXn0; //Werte überschreiben
                          AccYn3=AccYn0;
                          AccZn3=AccZn0;
                          AccTn3=AccTn0;
                          AcXXn3=AcXXn0;
                          AcYYn3=AcYYn0;
                          AcZZn3=AcZZn0;
                          
                        }
                      }
                    }
                  }
                }
              // rekursive Mittelwertberechnung
              AccMWX=(MAccXn0+AccXn1+AccXn2+AccXn3+AccXn4+AccXn5+AccXn6)/7;
              AccMWY=(MAccYn0+AccYn1+AccYn2+AccYn3+AccYn4+AccYn5+AccYn6)/7;
              AccMWZ=(MAccZn0+AccZn1+AccZn2+AccZn3+AccZn4+AccZn5+AccZn6)/7;
              AccMWXX=(MAcXXn0+AcXXn1+AcXXn2+AcXXn3+AcXXn4+AcXXn5+AcXXn6)/7;
              AccMWYY=(MAcYYn0+AcYYn1+AcYYn2+AcYYn3+AcYYn4+AcYYn5+AcYYn6)/7;      
              AccMWZZ=(MAcZZn0+AcZZn1+AcZZn2+AcZZn3+AcZZn4+AcZZn5+AcZZn6)/7;
              AccMWT=(MAccTn0+AccTn1+AccTn2+AccTn3+AccTn4+AccTn5+AccTn6)/7;

        //Ausgabe
        tft.fillRect(X_Ausgabe_Daten,0,1+Y_Schrittweite_Daten*11,200,BLUE);
        int Ctr=0;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccZn0);//AccMWXX);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccMWYY);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccMWZZ);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccMWX);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccMWY);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccMWZ);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(AccMWT);
        Ctr++;
        
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(GyrXn0);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(GyrYn0);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(GyrZn0);
        Ctr++;
        tft.setCursor(X_Ausgabe_Daten,1+Y_Schrittweite_Daten*Ctr);
        tft.println(GyrTn0);
        
        delay(200);                  
        
  
  
  
        
        
//        int AccdX=0;
//        int AccdY=0;
//        int AccdZ=0;
//        int AccdT=0;
        
        }}      
      break;
      
    }
}

Leider war es nicht möglich, aus dem Gyrosensor plausible Daten herauszubekommen. Sie blieben vom Beginn der Ausleseversuche unverändert bei einem recht hohen Wert, obwohl durchgehend mit 3,3 V Versorgungsspannung gearbeitet wurde. Anhand der Registerauslesung (grün =1, nicht gefärbt = 0) fallen keine besonderheiten auf. Der Sensor befindet sich in den Standardeinstellungen.

Die Register des Gyrosensors können allerdings beschrieben werden, was darauf schließen lässt, dass der Sensor nicht defekt ist, sondern lediglich eine art "Wake Up" Befehl benötigt. Evtl. liegt es auch an der Nichtbelegung von Pin1, dass der Sensor keine dynamischen Werte ausgibt.