Rotary scanner based on Ultrasonic sensor and TFT screen

#include <gfxfont.h>
#include <firFilter.h>
firFilter Filter;
int value;
int filtered;
// HACER QUE PINTE UNA VEZ Y HASTA QUE NO DETECTE LA TOUCH NO CAMBIE, O EL PUERTO SERIE
// LIMPIAR EL CÓDIGO NO USADO

#include "TouchScreen.h"
#include <Adafruit_GFX.h> // Hardware-specific library
#include <Adafruit_TFTLCD.h> // Hardware-specific library
#include <SPI.h>
#include <Servo.h>

//#include <SD.h>
//#define SD_CS  10  // CHIP SELECT CARTA SD DEL TFT

#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

// Pins for the LCD Shield
#define YP A3 // must be analog
#define XM A2 // must be analog
#define YM 9  // digital or analog pin
#define XP 8  // digital or analog pin

#define MINPRESSURE 1
#define MAXPRESSURE 1000

// Calibration mins and max for raw data when touching edges of screen
#define TS_MINX 210
#define TS_MINY 210
#define TS_MAXX 915
#define TS_MAXY 910

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


int sensorPin = A0;  
int sensorValue = 0; 
float sensorValor=0.00;
int n=0;

//Declaraciones para el Ultrasonidos
long tiempo;
int Trig = 13;      // triger
int Echo = 12;      // echo
//float Distancia;
float Dist,Dist1,Dist2;
Adafruit_TFTLCD tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);
int touchCnt = 0;
int buttonState = 0;
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
uint16_t x = 50, y = 100;

String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete
String nombre_fichero_ino = "TFT_TRACE6.INO";

void setup()
{
    //Configurar HC-SR04 Sensor Ultrasonidos.
    pinMode(Trig, OUTPUT);
    pinMode(Echo, INPUT);
    
  
    uint16_t tmp;     
    Serial.begin(9600);
     Filter.begin();
    Serial.print(F("[SBS:256,256][SPC:#FF0000,#00FF00]"));
   
    // reserve 200 bytes for the inputString:
    inputString.reserve(200);
    //reset tft y espera a que vuelva en sí
    tft.reset();
    delay(1000);
    tft.begin(9600);
    delay(1000);
    uint16_t identifier = tft.readID();
    Serial.print("El ID de la placa tft es = 0x");
    Serial.println(identifier, HEX);
    tft.begin(identifier);
    tft.fillScreen(BLACK);
  
    Serial.print(F("Initializing SD card..."));
    tft.setRotation(1);
    tft.setTextSize(2);
    tft.print("ID placa tft = 0x");
    tft.println(identifier, HEX);
    delay(300);
    tft.println("");
    tft.println(F("Inicializando carta SD..."));
    delay(200);

    //Verificamos si hay carta SD en el slot  
   /* if (!SD.begin(SD_CS))
    {
      //Si no está la SD
    tft.println("Error:No puedo acceder a la SD");
    Serial.println(F("Error:No puedo acceder a la SD"));
    return;
    }
    //Si SÍ está la SD
    Serial.println(F("OK!"));
    tft.print(F("...."));
    delay(10);
    tft.println(F(".............."));
    tft.println(F(""));
    tft.println(F("SD lista!"));
    tft.println("               ");
    tft.println(F(".............."));
    tft.println("               ");
    tft.println(nombre_fichero_ino);
    tft.println("               ");
    tft.println("rjuarez7@gmail.com");
  
    //No sé para qué es lo que sigue
    pinMode(13, OUTPUT);
 */
    delay(100); 
}
void loop()
{
 sonar();      
}
void sonar()
{
      float rad,r2;
      int x2,y2;
      r2=0.0;
      tft.fillScreen(BLACK);
      tft.drawPixel(160, 120, CYAN);
      tft.setCursor(67, 10);
      tft.drawRect(60, 5, 185,25,BLUE);
      tft.setTextSize(2);
      tft.setTextColor(RED);
      tft.println(nombre_fichero_ino);
      tft.fillRect(240,150,60,60,BLACK);
      tft.setTextSize(1);
      tft.setTextColor(YELLOW);
      tft.setCursor(250, 160);
      tft.print("X2= ");
      tft.setCursor(250, 170);
      tft.print("Y2= ");
      tft.print("Dis= ");
      tft.setCursor(250, 190);
      tft.setCursor(10, 200);
      tft.print("Barridos(n)= ");
      tft.fillRect(85,200,60,10,BLACK);
      tft.println(n); 
      tft.setTextSize(1);
      tft.setTextColor(GREEN);
      tft.setCursor(10, 220);
      tft.println("Conectar HC-SR04: Trig -> D13 y Echo -> D12");
      tft.setCursor(10, 228);
      tft.println("Motor orange -> D11 , red +5V y brown 0V");
      n++;
 while(1)  {  
 for (float i=0; i <= 360; i=i+1){
      ultrasonido(Dist); 
      delay(100);
      filtered= Filter.run(Dist);
      filtered=constrain(filtered,0,85);
      Serial.println(filtered);     
      r2=filtered;
      rad=i*2*PI/360;
      x2=r2*cos(rad);
      y2=r2*sin(rad);
      tft.drawLine(160,120, x2+160, y2+120, YELLOW); //DIBUJA DE MANERA POLAR LA DISTANCIA QUE MIDE EL SENSOR.
      tft.setCursor(250, 160);
      tft.setTextSize(1);
      tft.setTextColor(YELLOW);
      tft.fillRect(252,160,60,10,BLACK);
      tft.print("X2= ");
      tft.println(x2);
      tft.setCursor(250, 170);
      tft.fillRect(252,170,60,10,BLACK);
      tft.print("Y2= ");
      tft.println(y2);  
      } 
   tft.setCursor(10, 200);
   tft.print("Barridos(n)= ");
   tft.fillRect(85,200,60,10,BLACK);
   tft.println(n);
   n++;
   tft.fillCircle(160, 120, 90,BLACK);
   tft.drawPixel(160, 120, CYAN);
   }    
 }

/*Este módulo calcula y devuelve la distancia en cm.*/
void ultrasonido (float &Distancia){
//Para estabilizar el valor del pin Trig se establece a LOW
digitalWrite (Trig, LOW);
delay(10);
//Se lanzan los 8 pulsos
digitalWrite (Trig, HIGH);
delay(10);
digitalWrite (Trig, LOW);
/*
Se mide el tiempo que tarda la señal en regresar y se calcula la distancia.
Observa que al realizar pulseIn el valor que se obtiene es tiempo, no distancia
Se está reutilizando la variable Distancia.
*/
Distancia= pulseIn (Echo, HIGH);
Distancia=Distancia/58;
delay(10); 
}