Arduino touchscreen test

#include <SPFD5408_Adafruit_GFX.h>    // Core graphics library
#include <SPFD5408_Adafruit_TFTLCD.h> // Hardware-specific library
#include <SPFD5408_TouchScreen.h>
#include  <Chrono.h>

// LCD Pin

#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 LCD_RESET A4 // Can alternately just connect to Arduino's reset pin

#define YP A1
#define XM A2
#define YM 7
#define XP 6

// Assign human-readable names to some common 16-bit color values:
#define WHITE   0x0000
#define YELLOW    0x001F
#define CYAN     0xF800
#define MAGENTA   0x07E0
#define RED    0x07FF
#define GREEN 0xF81F
#define BLUE  0xFFE0
#define BLACK   0xFFFF

// Calibrates value
#define SENSIBILITY 300
#define MINPRESSURE 10
#define MAXPRESSURE 1000

//These are the pins for the shield!
#define YP A1
#define XM A2
#define YM 7
#define XP 6

short TS_MINX = 132;
short TS_MINY = 157;
short TS_MAXX = 976;
short TS_MAXY = 860;

int trigPin = 11;    //Trig - green Jumper
int echoPin = 10;    //Echo - yellow Jumper
long duration, cm = 0, inches;

Adafruit_TFTLCD tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);
TouchScreen ts = TouchScreen(XP, YP, XM, YM, SENSIBILITY);

Chrono screenUpdator;

void setup() {
  tft.reset();
  tft.begin(0x9341);
  tft.setRotation(0);
  tft.fillScreen(WHITE);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  tft.setTextColor(BLUE);
  tft.setTextSize(2);
  Serial.begin(9600);
  draw();
}

int v = 0 , y = 0, p = 0;
int color = RED;
boolean state = false;
void loop() {
  p = map(v, 5, 80, 0, 100);
  y = 310 - (v * 3);

  if (v <= 25)
    color = RED;
  else if (v <= 70)
    color = BLUE;
  else if (v > 70)
    color = GREEN;

  // printDistance();
  //  tft.setCursor(85, 50);
  //  tft.print(100 - p);
  //  tft.print(F("%"));

  if (screenUpdator.hasPassed(5000)) {
    draw();
  }

  TSPoint point = waitOneTouch();
  if (point.x >= 750 && point.x <= 920) {
    if (point.y >= 150 && point.y <= 650) {
      state = !state;
      draw();
    }
  }

  if (state) {
    Serial.println(1);
  } else {
    Serial.println(0);
  }

  if (state)
    v += 1;
  if (v >= 100)
    v = 0;
  delay(100);
}

void draw() {
  screenUpdator.restart();
  tft.fillScreen(BLACK);
  tft.drawRect(10, 10, 50, 300, WHITE);

  tft.setTextColor(color);
  tft.fillRect(15, y, 40, 300 - y, color);

  // status text
  tft.setTextSize(6);
  tft.setCursor(80, 30);
  tft.print(v);
  tft.print(F("%"));

  // On button
  tft.setTextSize(2);
  tft.setCursor(130, 265);
  if (state) {
    tft.setTextColor(GREEN);
    tft.print(F("ON"));
    tft.drawRect(70, 235, 160, 75, GREEN);
  } else {
    tft.setTextColor(RED);
    tft.print(F("OFF"));
    tft.drawRect(70, 235, 160, 75, RED);
  }

}

TSPoint waitOneTouch() {

  // wait 1 touch to exit function

  TSPoint p;

  //  do {
  p = ts.getPoint();

  pinMode(XM, OUTPUT); //Pins configures again for TFT control
  pinMode(YP, OUTPUT);

  //  } while ((p.z < MINPRESSURE ) || (p.z > MAXPRESSURE));

  return p;
}

void printDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

  // convert the time into a distance
  cm = (duration / 2) / 29.1;
  inches = (duration / 2) / 74;
  //tft.print(cm);
  //tft.println(F("cm"));
}