//////////////////////////////////////////////////////////////////////// 
///

#include <stdint.h>
#include "TouchScreen.h"
#include <SPI.h>
#include <Adafruit_GFX.h>
#include "Adafruit_HX8357.h"
#include <MCUFRIEND_kbv.h>
#define TFT_RST 8
#define TFT_DC 9
#define TFT_CS 10
#define YP A1
#define XM A0
#define YM 28
#define XP 29

TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
Adafruit_HX8357 tft = Adafruit_HX8357(TFT_CS, TFT_DC, TFT_RST);

#include <Fonts/FreeSans9pt7b.h>
#include <Fonts/FreeSans12pt7b.h>
#include <Fonts/FreeSerif12pt7b.h>
#include <FreeDefaultFonts.h>
#include <Fonts/FreeSansBold18pt7b.h>
#include <Fonts/FreeSansBold24pt7b.h>
#define width   320
#define height  480
#define BLACK   0x0000
#define RED     0xF800
#define GREEN   0x07E0
#define BLUE    0x000F
#define WHITE   0xFFFF
#define GREY    0x8410
#define YELLOW  0xFFE0


int ID;
int setID;
int CANID;
int Menu = 0;
int Return = 0;
int setPID;
int sBY1  , rB;
int Byte0,ByteDisp, bitDisp;
int Bit0, Bit1, Bit2, Bit3 ,Bit4 ,Bit5 ,Bit6, Bit7;
int aBit0, aBit1, aBit2, aBit3 ,aBit4 ,aBit5 ,aBit6, aBit7;
//int Bit0a, Bit1a, Bit2a, Bit3a ,Bit4a ,Bit5a ,Bit6a, Bit7a;
//int aBit0a, aBit1a, aBit2a, aBit3a ,aBit4a ,aBit5a, aBit6a, aBit7a;

int  setPIDcoose ;

int alt;



#include "mcp_can.h"
const int SPI_CS_PIN = 7;
MCP_CAN CAN(SPI_CS_PIN);



void CANBUS(){
   tft.fillCircle(300,15,5,RED);
    unsigned char len = 0;
    unsigned char buf[8];
    if(CAN_MSGAVAIL == CAN.checkReceive())   {           // check if 
data coming

        CAN.readMsgBuf(&len, buf);    // read data,  len: data length, 
buf: data buf

        unsigned long canId = CAN.getCanId();
        ID = canId;
        if ( ID == setID){
          tft.fillCircle(300,15,5,GREEN);

        for(int i = 0; i<len; i++)    // print the data
        {
          if (ByteDisp == 1){
    tft.fillRect(15,138,50,30,BLACK);
    tft.setCursor(20,160);
    tft.print(buf[0], DEC);
    tft.fillRect(93,138,50,30,BLACK);
    tft.setCursor(98,160);
    tft.print(buf[1], DEC);
    tft.fillRect(173,138,50,30,BLACK);
    tft.setCursor(178,160);
    tft.print(buf[2], DEC);
    tft.fillRect(253,138,50,30,BLACK);
    tft.setCursor(258,160);
    tft.print(buf[3], DEC);
    tft.fillRect(15,218,50,30,BLACK);
    tft.setCursor(20,240);
    tft.print(buf[4], DEC);
    tft.fillRect(93,218,50,30,BLACK);
    tft.setCursor(98,240);
    tft.print(buf[5], DEC);
    tft.fillRect(173,218,50,30,BLACK);
    tft.setCursor(178,240);
    tft.print(buf[6], DEC);
    tft.fillRect(253,218,50,30,BLACK);
    tft.setCursor(258,240);
    tft.print(buf[7], DEC);}
     else{}
    }
      if ( bitDisp == 1 ){
           int j; j = sBY1;
    Byte0 = (buf[j]);
    Bit0 = bitRead(Byte0,0);
      Bit1 = bitRead(Byte0,1);
        Bit2 = bitRead(Byte0,2);
          Bit3 = bitRead(Byte0,3);
            Bit4 = bitRead(Byte0,4);
              Bit5 = bitRead(Byte0,5);
                Bit6 = bitRead(Byte0,6);
                  Bit7 = bitRead(Byte0,7);}
                 tft.fillCircle(300,15,5,BLACK);
                  }
          //  else{}
            if ( bitDisp == 1 ){
                BITDisplay(); }
                else{}
           aBit0 = Bit0; aBit1 = Bit1; aBit2 = Bit2; aBit3 = Bit3;
           aBit4 = Bit4; aBit5 = Bit5; aBit6 = Bit6; aBit7 = Bit7;
            }
       tft.fillCircle(300,15,5,BLACK);
    }



void setup() {
  Serial.begin(9600);
    tft.begin();
  tft.setRotation(0);
  tft.fillScreen(BLACK);
  tft.setFont(&FreeSerif12pt7b);
  tft.setTextSize(1);
  tft.setTextColor(GREEN);
  Serial.print("Initializing TFT");
  tft.setCursor(20, 100);
  tft.print("CANBUS Reader A4 B5 AJM ");
  delay(1000);
   tft.fillScreen(BLACK);
       while (CAN_OK != CAN.begin(CAN_500KBPS))              // init can 
bus : baudrate = 500k
    {
        Serial.println("CAN BUS Shield init fail");
        Serial.println(" Init CAN BUS Shield again");
        delay(100);
    }
    Serial.println("CAN BUS Shield init ok!");
   updateMenu();
}




void BITRead() {                    //sBY1
  tft.setCursor(15,40); tft.print("PID 0x");
  String dCANID = String(CANID);
  tft.setCursor(90,40); tft.print(CANID,HEX);
  tft.setCursor(190,40); tft.print("Byte:");
  tft.setCursor(260,40);tft.print(sBY1);
  tft.setCursor(10,120);
  tft.print("7   6    5   4    3   2    1 /   0 ");

}
void BitReadAktive() {
   tft.drawRect(15,138,50,30,WHITE); 
tft.drawRect(93,138,50,30,WHITE);tft.drawRect(173,138,50,30,WHITE);tft.d 
rawRect(253,138,50,30,WHITE);
   tft.drawRect(15,218,50,30,WHITE); 
tft.drawRect(93,218,50,30,WHITE);tft.drawRect(173,218,50,30,WHITE);tft.d 
rawRect(253,218,50,30,WHITE);
}
void Touch() {
   TSPoint p = ts.getPoint();

  if( p.z > ts.pressureThreshhold){
      //Serial.print("x: "); Serial.println(p.x);
      //Serial.print("y: "); Serial.println(p.y);
      //Serial.print("z: "); Serial.println(p.z);
      }
}

void loop() {
  //Serial.println("****************************************************** 
*****************************************");
  //Serial.println("void loop()");
 // Serial.print("sBY1"); Serial.println(sBY1);
  //Serial.print("setID"); Serial.println(setID);
    //Serial.print("rB :"); Serial.println(rB);
   //Serial.print("Menu: ");Serial.println(Menu);
   //Serial.print("bitDisp"); Serial.println(bitDisp);
  //Serial.print("setPIDcoose= "); Serial.println(setPIDcoose);
   CANBUS();
  tft.setCursor(35,440);
  tft.print("Back");
  tft.drawRect(30,415,60,40,WHITE);
  if ( setPIDcoose == 1){
      PIDCoose();
    }
  if ( rB == 1){
      TouchActiveKeyBitRead();
      TouchBitselect();
      }
  if ( rB == 2){
      TouchBitselect();
      }
  if ( Return != 0){
      TouchReturn(); }
  CANBUS();
  //Serial.print("rBI: "); Serial.print(rB);
}

void updateMenu() {
  //Serial.print("void update Menu: ");
  //Serial.println(Menu);
  switch(Menu){
  case 0:
    Menu==0;
    tft.fillScreen(BLACK);
    ByteDisp = 0;
    bitDisp = 0;
    CANMiniCooperS();
    setPIDcoose = 1;
    break;

  case 1:
    Menu==1;

      tft.fillScreen(BLACK);
      rB = 1;   // ggf 1
      ByteDisp = 1;
      Return = 1;
      MINIPID();

    break;

  case 2:
      Menu==2;
      tft.fillScreen(BLACK);
      ByteDisp = 1;
      ByteDisp = 0;
      BITRead();
      Return = 2;

    break;

 case 3:
      Menu==3;
       delay(300);
       ByteDisp = 1;
      bitDisp = 0;
      rB=2;
      Return = 3;
    break;

  case 4:
      Menu==4;
      tft.fillScreen(BLACK);
      rB=0;
      ByteDisp = 0;
       bitDisp = 1;
      BITRead();
      Return = 4;
    break;
  }
}

void CANMiniCooperS() {
  tft.setCursor(55,40);
  tft.print("PID Audi A4 AJM");
  tft.setCursor(40,85);
  tft.print("280");
  tft.drawRect(30,60,60,40,RED);
  tft.setCursor(130,85);
  tft.print("288");
  tft.drawRect(120,60,60,40,RED);
  tft.setCursor(220,85);
  tft.print("320");
  tft.drawRect(210,60,60,40,RED);

  tft.setCursor(40,155);
  tft.print("380");
  tft.drawRect(30,130,60,40,RED);
  tft.setCursor(130,155);
  tft.print("388");
  tft.drawRect(120,130,60,40,RED);
  tft.setCursor(220,155);
  tft.print("420");
  tft.drawRect(210,130,60,40,RED);

  tft.setCursor(40,225);
  tft.print("480");
  tft.drawRect(30,200,60,40,RED);
  tft.setCursor(130,225);
  tft.print("488");
  tft.drawRect(120,200,60,40,RED);
  tft.setCursor(220,225);
  tft.print("520");
  tft.drawRect(210,200,60,40,RED);

  tft.setCursor(40,295);
  tft.print("588");
  tft.drawRect(30,270,60,40,RED);
  tft.setCursor(130,295);
  tft.print("01A");
  tft.drawRect(120,270,60,40,RED);
  tft.setCursor(220,295);
  tft.print("04A");
  tft.drawRect(210,270,60,40,RED);

  tft.setCursor(40,365);
  tft.print("05A");
  tft.drawRect(30,340,60,40,RED);
  tft.setCursor(130,365);
  tft.print("---");
  tft.drawRect(120,340,60,40,RED);
  tft.setCursor(220,365);
  tft.print("---");
  tft.drawRect(210,340,60,40,RED);

  tft.setCursor(130,440);
  tft.print("---");
  tft.drawRect(120,415,60,40,RED);
}

void PIDCoose() {
    TSPoint p = ts.getPoint();
        if( p.z > ts.pressureThreshhold){


      if (Menu == 0 && (p.x>195) && (p.x<332) && (p.y>170) && 
(p.y<235)){

   setPID = 1; setID = 640; CANID = 0x280; 
tft.drawRect(30,60,60,40,GREEN); // 1AA
   delay(500); Menu = 1;updateMenu();}
      if (Menu == 0 && (p.x>404) && (p.x<570) && (p.y>170) && 
(p.y<235)){

   setPID = 2; setID = 648; CANID = 0x288; 
tft.drawRect(120,60,60,40,GREEN); // 1F0
   delay(500); Menu = 1;updateMenu();}
      if (Menu == 0 && (p.x>650) && (p.x<800) && (p.y>170) && 
(p.y<235)){

   setPID = 3; setID = 800; CANID = 0x320; 
tft.drawRect(210,60,60,40,GREEN);// 1F3
   delay(500); Menu = 1;updateMenu();}
// Row 2
     if (Menu == 0 && (p.x>195) && (p.x<332) && (p.y>292) && (p.y<370)){

   setPID = 4; setID = 896; CANID = 0x380; 
tft.drawRect(30,130,60,40,GREEN); // 1F5
   delay(500); Menu = 1; updateMenu();}
   if (Menu == 0 && (p.x>404) && (p.x<570) && (p.y>292) && (p.y<370)){

   setPID = 5; setID = 904; CANID = 0x388; 
tft.drawRect(120,130,60,40,GREEN);//1F8
   delay(500); Menu = 1;updateMenu();}
   if (Menu == 0 && (p.x>650) && (p.x<800) && (p.y>292) && (p.y<370)){

   setPID = 6; setID = 1056; CANID = 0x420; 
tft.drawRect(210,130,60,40,GREEN);// 1A8
   delay(500); Menu = 1;updateMenu();}

// Row 3

     if (Menu == 0 && (p.x>195) && (p.x<332) && (p.y>418) && (p.y<486)){

   setPID = 7; setID = 1152; CANID = 0x480; 
tft.drawRect(30,200,60,40,GREEN); // 61A
   delay(500); Menu = 1; updateMenu();}
   if (Menu == 0 && (p.x>404) && (p.x<570) && (p.y>418) && (p.y<486)){

   setPID = 8; setID = 1160; CANID = 0x488; 
tft.drawRect(120,200,60,40,GREEN);//153
   delay(500); Menu = 1;updateMenu();}
   if (Menu == 0 && (p.x>650) && (p.x<800) && (p.y>418) && (p.y<486)){

   setPID = 9; setID = 1312; CANID = 0x520; 
tft.drawRect(210,200,60,40,GREEN);// 316
   delay(500); Menu = 1;updateMenu();}

// Row 4

     if (Menu == 0 && (p.x>195) && (p.x<332) && (p.y>547) && (p.y<612)){

   setPID = 10; setID = 1416; CANID = 0x588; 
tft.drawRect(30,270,60,40,GREEN); // 329
   delay(500); Menu = 1; updateMenu();}
   if (Menu == 0 && (p.x>404) && (p.x<570) && (p.y>547) && (p.y<612)){

   setPID = 11; setID = 416; CANID = 0x01A; 
tft.drawRect(120,270,60,40,GREEN);//545
   delay(500); Menu = 1;updateMenu();}
   if (Menu == 0 && (p.x>650) && (p.x<800) && (p.y>547) && (p.y<612)){

   setPID = 12; setID = 1184; CANID = 0x04A; 
tft.drawRect(210,270,60,40,GREEN);// 565
   delay(500); Menu = 1;updateMenu();}

//Row 5

   if (Menu == 0 && (p.x>195) && (p.x<332) && (p.y>678) && (p.y<740)){

   setPID = 13; setID = 1440; CANID = 0x05A; 
tft.drawRect(30,340,60,40,GREEN); // 615
   delay(500); Menu = 1; updateMenu();}
   if (Menu == 0 && (p.x>404) && (p.x<570) && (p.y>678) && (p.y<740)){

   setPID = 14; setID = 1557; CANID = 0x615; 
tft.drawRect(120,340,60,40,GREEN);// 618
   delay(500); Menu = 1;updateMenu();}
   if (Menu == 0 && (p.x>650) && (p.x<800) && (p.y>678) && (p.y<740)){

   setPID = 15; setID = 1560; CANID = 0x618; 
tft.drawRect(210,340,60,40,GREEN);// 565
   delay(500); Menu = 1; updateMenu();}
    } //setCar3

// Row 6

    if (Menu == 0 && (p.x>404) && (p.x<570) && (p.y>820) && (p.y<885)){

   setPID = 16; setID = 1562; CANID = 
0x61A;tft.drawRect(120,415,60,40,GREEN); // 61A
   delay(500); Menu = 1;updateMenu();}

}

void MINIPID() {

  tft.setCursor(20,40);
  tft.print("Audi A4 B5 AJM PID *");
  tft.setCursor(20,80);
      if( setPID == 1){
  tft.print("PID:    280       DEC:");}
      if( setPID == 2){
  tft.print("PID:    288       DEC:");}
      if( setPID == 3){
  tft.print("PID:    320       DEC:");}
      if( setPID == 4){
  tft.print("PID:    380       DEC:");}
      if( setPID == 5){
  tft.print("PID:    388       DEC:");}
      if( setPID == 6){
  tft.print("PID:    420       DEC:");}
      if( setPID == 7){
  tft.print("PID:    480       DEC:");}
      if( setPID == 8){
  tft.print("PID:    488       DEC:");}
      if( setPID == 9){
  tft.print("PID:    520       DEC:");}
      if( setPID == 10){
  tft.print("PID:    588       DEC:");}
      if( setPID == 11){
  tft.print("PID:    01A       DEC:");}
      if( setPID == 12){
  tft.print("PID:    04A       DEC:");}
      if( setPID == 13){
  tft.print("PID:    05A       DEC:");}
      if( setPID == 14){
  tft.print("PID:    ---       DEC:");}
      if( setPID == 15){                    //***
  tft.print("PID:    ---       DEC:");}
      if( setPID == 16){
  tft.print("PID:    ---       DEC:");}
  tft.setCursor(20,120);
  tft.print("buf0     buf1     buf2     buf3");
  tft.setCursor(20,200);
  tft.print("buf4     buf5     buf6     buf7");
  tft.setCursor(20,160);

    //tft.drawRect(15,138,50,30,WHITE); 
tft.drawRect(93,138,50,30,WHITE);tft.drawRect(173,138,50,30,WHITE);tft.d 
rawRect(253,138,50,30,WHITE);
    tft.setCursor(20,160);
    tft.print("---");
     tft.setCursor(98,160);
    tft.print("---");
    tft.setCursor(178,160);
    tft.print("---");
    tft.setCursor(258,160);
    tft.print("---");
   // tft.drawRect(15,218,50,30,WHITE); 
tft.drawRect(93,218,50,30,WHITE);tft.drawRect(173,218,50,30,WHITE);tft.d 
rawRect(253,218,50,30,WHITE);
    tft.setCursor(20,240);
    tft.print("---");
    tft.setCursor(98,240);
    tft.print("---");
    tft.setCursor(178,240);
    tft.print("---");
    tft.setCursor(258,240);
    tft.print("---");

  tft.setCursor(40,330);
  tft.print("set byte*");
  tft.drawRect(20,300,120,50,WHITE);

  tft.setCursor(200,330);
  tft.print("BitRead*");
  tft.drawRect(180,300,120,50,WHITE);

}



void BITDisplay() {                 //Bit0, Bit1, Bit2, Bit3 ,Bit4 ,Bit5 
,Bit6, Bit7;

   if (Bit7 != aBit7){
   tft.setTextColor(BLACK);tft.setCursor(10,160);tft.print(aBit7);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(10,160);tft.print(Bit7);
   }
   if (Bit6 != aBit6){
   tft.setTextColor(BLACK);tft.setCursor(50,160);tft.print(aBit6);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(50,160);tft.print(Bit6);
   }
   if (Bit5 != aBit5){
   tft.setTextColor(BLACK);tft.setCursor(90,160);tft.print(aBit5);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(90,160);tft.print(Bit5);
   }
   if (Bit4 != aBit4){
   tft.setTextColor(BLACK);tft.setCursor(130,160);tft.print(aBit4);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(130,160);tft.print(Bit4);
   }
   if (Bit3 != aBit3){
   tft.setTextColor(BLACK);tft.setCursor(170,160);tft.print(aBit3);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(170,160);tft.print(Bit3);
   }
   if (Bit2 != aBit2){
   tft.setTextColor(BLACK);tft.setCursor(210,160);tft.print(aBit2);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(210,160);tft.print(Bit2);
   }
   if (Bit1 != aBit1){
   tft.setTextColor(BLACK);tft.setCursor(250,160);tft.print(aBit1);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(250,160);tft.print(Bit1);
   }
   if (Bit0 != aBit0){
   tft.setTextColor(BLACK);tft.setCursor(290,160);tft.print(aBit0);
   delay(5);
   tft.setTextColor(WHITE);tft.setCursor(290,160);tft.print(Bit0);
   }
}

 void TouchActiveKeyBitRead() {

  TSPoint p = ts.getPoint();
  if( p.z > ts.pressureThreshhold){
     if ((p.x>565) && (p.x<855) && (p.y>610) && (p.y<692)){ //  set byte

   BitReadAktive();
  tft.setCursor(200,330);
  tft.setTextColor(BLUE);
  tft.print("BitRead");
  tft.setTextColor(WHITE),
  tft.drawRect(180,300,120,50,BLUE);
  Menu = 3;
  }
  // Menu 11
   if (Menu == 3 &&(sBY1 != 8) && (p.x>154) && (p.x<450) && (p.y>604) && 
(p.y<690)){ // key set byte

  // setPID = 30;
  tft.drawRect(20,300,120,50,GREEN);// buf 0
  tft.setCursor(40,330);
  tft.setTextColor(RED);
  tft.print("set byte");
  delay(200);
  tft.setCursor(40,330);
  tft.setTextColor(WHITE);
  tft.print("set byte");
  Menu = 4; updateMenu();}
}
}

void TouchBitselect() {
  TSPoint p = ts.getPoint();
      if( p.z > ts.pressureThreshhold){
if(rB == 1 && Menu == 3){
   if ((p.x>145) && (p.x<260) && (p.y>305) && (p.y<355)){ //  set byte
   sBY1 = 0;
  // setPID = 30;
  tft.drawRect(15,138,50,30,GREEN);;// buf 0
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
   if ((p.x>330) && (p.x<470) && (p.y>305) && (p.y<355)){ //  set byte
   sBY1 = 1;
  // setPID = 30;
  tft.drawRect(93,138,50,30,GREEN);// buf 1
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
     if ((p.x>540) && (p.x<670) && (p.y>305) && (p.y<355)){ //  set byte
   sBY1 = 2;
  // setPID = 30;
  tft.drawRect(173,138,50,30,GREEN);// buf 2
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
     if ((p.x>743) && (p.x<870) && (p.y>305) && (p.y<355)){ //  set byte
   sBY1 = 3;
  // setPID = 30;
  tft.drawRect(253,138,50,30,GREEN);// buf 3
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
    if ((p.x>140) && (p.x<255) && (p.y>450) && (p.y<500)){ //  set byte

   sBY1 = 4;
  // setPID = 30;
  tft.drawRect(15,218,50,30,GREEN);// buf 4
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
       if ((p.x>340) && (p.x<460) && (p.y>450) && (p.y<500)){ //  set 
byte

   sBY1 = 5;
  // setPID = 30;
  tft.drawRect(93,218,50,30,GREEN);// buf 5
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
      if ((p.x>540) && (p.x<665) && (p.y>450) && (p.y<500)){ //  set 
byte

   sBY1 = 6;
  // setPID = 30;
  tft.drawRect(173,218,50,30,GREEN);// buf 6
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }

   if ((p.x>765) && (p.x<880) && (p.y>450) && (p.y<500)){ //  set byte

   sBY1 = 7;
  // setPID = 30;
  tft.drawRect(253,218,50,30,GREEN);// buf 7
  delay(200);
  tft.drawRect(20,300,120,50,BLUE);
  delay(200);
  Menu = 4;updateMenu();
  }
        } // if rBI == 1
}
}

void TouchReturn() {
  TSPoint p = ts.getPoint();
     if ( p.z > ts.pressureThreshhold){ //Retun all planes to 0
  if( (p.x>182) && (p.x<325) && (p.y>315) && (p.y<880)){
    p.x = 0; p.y = 0; p.z = 0;
    tft.drawRect(30,415,60,40,RED);
     if ( Return == 1){
    Menu = 0;}
     if ( Return == 2){
    Menu = 1;}
     if ( Return == 3){
    Menu = 2;}
     if ( Return == 4){
    Menu = 2;}
    Zero();
    bitDisp = 0;
    Return = 0;
    setID = 0;
    rB = 0;
    delay(500);
    updateMenu();
     }
     }

   if (Return == 2  &&( p.z > ts.pressureThreshhold)){ //Retun all 
planes
  if( (p.x>230) && (p.x<790) && (p.y>780) && (p.y<860)){
     p.x = 0; p.y = 0; p.z = 0;
    tft.drawRect(50,400,220,50,RED);
    Menu = 3;
    Zero();
    Return = 0;
    setID = 0;
    rB = 0;
    delay(500);
    updateMenu();
     }
     }
      else{}
}

void Zero() {

aBit0 = 2; aBit1 = 2; aBit2 = 2; aBit3 = 2;  aBit4 = 2;  aBit5 = 2; 
aBit6 = 2; aBit7 = 2;
}
//Serial.println("*******************TEST*******************");