Rotary scanner using Arduino MEGA2560 and TFT

#include <gfxfont.h>
#include <firFilter.h>
firFilter Filter;
int value;
int filtered;
#include "TouchScreen.h"  // C:\Program Files (x86)\Arduino\libraries\Touch-Screen-Library-master
#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  53  // CHIP SELECT CARTA SD DEL TFT USANDO MEGA

#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 A2 // must be analog
#define XM A3 // must be analog
#define YM 8  // digital or analog pin
#define XP 9  // 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

//Declaraciones para el Sensor TCRT5000
int sensorTCRT5000 = A15;  
int sensorValue = 0; 
float sensorValor=0.00;
int n=0;

//Declaraciones para el Ultrasonidos
long tiempo;
int Trig = 11;      // triger
int Echo = 12;      // echo

//Declaraciones para el Motor
int motor = 13;
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 = "TFTMEGA.INO";

void setup()
{
    //Configurar HC-SR04 Sensor Ultrasonidos.
    pinMode(Trig, OUTPUT);
    pinMode(Echo, INPUT);
    
    pinMode(motor, OUTPUT);
  
    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(50);
    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");
   bmpDraw("4.bmp", 0, 0);

 
    delay(100); 
}
void loop()
{
 //tactil();
 //delay(1000);
 sonar();      
}
void sonar()
{
      float rad,r2;
      int x2,y2;
      r2=0.0;
      digitalWrite (motor, LOW);
      delay(500);
      tft.fillScreen(BLACK);
        bmpDraw("1.bmp", 270, 5);
        bmpDraw("12.bmp", 5, 5);
      tft.drawPixel(160, 120, CYAN);
      tft.setCursor(94, 10);
      tft.drawRect(69, 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.setCursor(250, 190);
      tft.print("Dis= ");
      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 -> D11 y Echo -> D12");
      tft.setCursor(10, 228);
      tft.println("Motor orange -> D13 , 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;
      analogWrite(motor,255/r2);  // el led 13 se iluminams si se acerca objeto al sonar
      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(250, 190);
      tft.fillRect(250,190,60,10,BLACK);
      tft.setTextColor(WHITE);
      tft.print("Dis= ");
      tft.println(r2);
      tft.setCursor(250, 200);
      tft.fillRect(250,200,80,10,BLACK);
      tft.setTextColor(CYAN);
      tft.print("TCRT5= ");
      tft.println( analogRead(sensorTCRT5000));
      //Serial.print(" A5=");Serial.print(analogRead(A5));Serial.print(" A4=");Serial.print(analogRead(A4));Serial.print(" A3=");Serial.print(analogRead(A3));Serial.print(" A2=");Serial.print(analogRead(A2));    Serial.print(" A1=");Serial.print(analogRead(A1));Serial.print(" A0=");Serial.println(analogRead(A0));    
      } 
   tft.setCursor(10, 200);
   tft.print("Barridos(n)= ");
   delay(100);
   tft.fillRect(85,200,60,10,BLACK);
   tft.println(n);
   n++;
   tft.fillCircle(160, 120, 90,BLACK);
   tft.drawPixel(160, 120, CYAN);
   }    
 }

/*Este mdulo 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 seal 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); 
}

void tactil()
{

  TSPoint p = ts.getPoint();
  // we have some minimum pressure we consider 'valid'
  // pressure of 0 means no pressing!
  if (p.z > ts.pressureThreshhold) {
     Serial.print("X = "); Serial.print(p.x);
     Serial.print("\tY = "); Serial.print(p.y);
     Serial.print("\tPressure = "); Serial.println(p.z);
      tft.setCursor(250, 160);
      tft.setTextSize(1);
      tft.setTextColor(YELLOW);
      tft.fillRect(252,160,60,10,BLACK);
      tft.print("X2= ");
      tft.println(p.x);
      tft.setCursor(250, 170);
      tft.fillRect(252,170,60,10,BLACK);
      tft.print("Y2= ");
      tft.println(p.y); 
      tft.setCursor(250, 190);
      tft.print("Dis= ");
      tft.println(p.z); 
     
  }

  delay(100);
}


/////////////////////////////////////////////////////
// This function opens a Windows Bitmap (BMP) file and
// displays it at the given coordinates.  It's sped up
// by reading many pixels worth of data at a time
// (rather than pixel by pixel).  Increasing the buffer
// size takes more of the Arduino's precious RAM but
// makes loading a little faster.  20 pixels seems a
// good balance.

#define BUFFPIXEL 20

void bmpDraw(char *filename, int x, int y) {

  File     bmpFile;
  int      bmpWidth, bmpHeight;   // W+H in pixels
  uint8_t  bmpDepth;              // Bit depth (currently must be 24)
  uint32_t bmpImageoffset;        // Start of image data in file
  uint32_t rowSize;               // Not always = bmpWidth; may have padding
  uint8_t  sdbuffer[3*BUFFPIXEL]; // pixel in buffer (R+G+B per pixel)
  uint16_t lcdbuffer[BUFFPIXEL];  // pixel out buffer (16-bit per pixel)
  uint8_t  buffidx = sizeof(sdbuffer); // Current position in sdbuffer
  boolean  goodBmp = false;       // Set to true on valid header parse
  boolean  flip    = true;        // BMP is stored bottom-to-top
  int      w, h, row, col;
  uint8_t  r, g, b;
  uint32_t pos = 0, startTime = millis();
  uint8_t  lcdidx = 0;
  boolean  first = true;

  if((x >= tft.width()) || (y >= tft.height())) return;

  Serial.println();
  Serial.print(F("Loading image '"));
  Serial.print(filename);
  Serial.println('\'');
  // Open requested file on SD card
  if ((bmpFile = SD.open(filename)) == NULL) {
    Serial.println(F("File not found"));
    return;
  }

  // Parse BMP header
  if(read16(bmpFile) == 0x4D42) { // BMP signature
    Serial.println(F("File size: ")); Serial.println(read32(bmpFile));
    (void)read32(bmpFile); // Read & ignore creator bytes
    bmpImageoffset = read32(bmpFile); // Start of image data
    Serial.print(F("Image Offset: ")); Serial.println(bmpImageoffset, DEC);
    // Read DIB header
    Serial.print(F("Header size: ")); Serial.println(read32(bmpFile));
    bmpWidth  = read32(bmpFile);
    bmpHeight = read32(bmpFile);
    if(read16(bmpFile) == 1) { // # planes -- must be '1'
      bmpDepth = read16(bmpFile); // bits per pixel
      Serial.print(F("Bit Depth: ")); Serial.println(bmpDepth);
      if((bmpDepth == 24) && (read32(bmpFile) == 0)) { // 0 = uncompressed

        goodBmp = true; // Supported BMP format -- proceed!
        Serial.print(F("Image size: "));
        Serial.print(bmpWidth);
        Serial.print('x');
        Serial.println(bmpHeight);

        // BMP rows are padded (if needed) to 4-byte boundary
        rowSize = (bmpWidth * 3 + 3) & ~3;

        // If bmpHeight is negative, image is in top-down order.
        // This is not canon but has been observed in the wild.
        if(bmpHeight < 0) {
          bmpHeight = -bmpHeight;
          flip      = false;
        }

        // Crop area to be loaded
        w = bmpWidth;
        h = bmpHeight;
        if((x+w-1) >= tft.width())  w = tft.width()  - x;
        if((y+h-1) >= tft.height()) h = tft.height() - y;

        // Set TFT address window to clipped image bounds
        tft.setAddrWindow(x, y, x+w-1, y+h-1);

        for (row=0; row<h; row++) { // For each scanline...
          // Seek to start of scan line.  It might seem labor-
          // intensive to be doing this on every line, but this
          // method covers a lot of gritty details like cropping
          // and scanline padding.  Also, the seek only takes
          // place if the file position actually needs to change
          // (avoids a lot of cluster math in SD library).
          if(flip) // Bitmap is stored bottom-to-top order (normal BMP)
            pos = bmpImageoffset + (bmpHeight - 1 - row) * rowSize;
          else     // Bitmap is stored top-to-bottom
            pos = bmpImageoffset + row * rowSize;
          if(bmpFile.position() != pos) { // Need seek?
            bmpFile.seek(pos);
            buffidx = sizeof(sdbuffer); // Force buffer reload
          }

          for (col=0; col<w; col++) { // For each column...
            // Time to read more pixel data?
            if (buffidx >= sizeof(sdbuffer)) { // Indeed
              // Push LCD buffer to the display first
              if(lcdidx > 0) {
                tft.pushColors(lcdbuffer, lcdidx, first);
                lcdidx = 0;
                first  = false;
              }
              bmpFile.read(sdbuffer, sizeof(sdbuffer));
              buffidx = 0; // Set index to beginning
            }

            // Convert pixel from BMP to TFT format
            b = sdbuffer[buffidx++];
            g = sdbuffer[buffidx++];
            r = sdbuffer[buffidx++];
            lcdbuffer[lcdidx++] = tft.color565(r,g,b);
          } // end pixel
        } // end scanline
        // Write any remaining data to LCD
        if(lcdidx > 0) {
          tft.pushColors(lcdbuffer, lcdidx, first);
        } 
        Serial.print(F("Loaded in "));
        Serial.print(millis() - startTime);
        Serial.println(" ms");
      } // end goodBmp
    }
  }

  bmpFile.close();
  if(!goodBmp) Serial.println(F("BMP format not recognized."));
}

// These read 16- and 32-bit types from the SD card file.
// BMP data is stored little-endian, Arduino is little-endian too.
// May need to reverse subscript order if porting elsewhere.

uint16_t read16(File f) {
  uint16_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read(); // MSB
  return result;
}

uint32_t read32(File f) {
  uint32_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read();
  ((uint8_t *)&result)[2] = f.read();
  ((uint8_t *)&result)[3] = f.read(); // MSB
  return result;
}