Trigonometric Height Measurement with 3D Magnetic Sensor

#include <SPI.h>
#include "epd1in54.h"
#include "epdif.h"
#include "epdpai#include <SPI.h>
#include "epd1in54.h"
#include "epdif.h"
#include "epdpaint.h"
#include <Tle493d_w2b6.h>
#include <RTC.h>
#include <DeviceControlXMC.h>
#include "epdpaint.h"


int TRIGpin = 14;
int ECHOpin = 8;
int SWCHpin = 13;


int COLORED  = 0; // White in Black
int UNCOLORED = 1;  // Black in White
int pos = 20;

XMCClass xmc2go;

// TLV493 variables //
float Xm = 0.0;
float Ym = 0.0;
float Zm = 0.0;
float angleD = 0.0; // angle in degree
float angleR = 0.0; // angle in radian
long adjacent = 0.0; // base to device distance + offset 5 cm
long offset =5.0;
float height = 0.0; // height of wall

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

unsigned char image[8192];   // takes 8k RAM 
//////////////////////////////////////////////////
// width must be multiple of 8
Paint paint(image, 0, 0);     
Epd epd;                     
Tle493d_w2b6 Tle493dMagnetic3DSensor = Tle493d_w2b6();
//////////////////////////////////////////////////////

void setup() 
{

pinMode (TRIGpin,OUTPUT);
pinMode (ECHOpin,INPUT);
pinMode (SWCHpin,INPUT);

///// Init Full Display Update ///////////
  if (epd.Init(lut_full_update) != 0) 
  {return;}
/////////////////////////////////////////////
  
///// Init Partial Display Update ////////
  if (epd.Init(lut_partial_update) != 0) 
  {return;}
//////////////////////////////////////////
////////// Clears Up Full Disp ///////////
// bit set = white, bit reset = black
  epd.ClearFrameMemory(0xFF);   
  epd.DisplayFrame();
  epd.ClearFrameMemory(0xFF);   
  epd.DisplayFrame();
//////////////////////////////////////////

}



void loop() 
{
  /// Get Mag sensor running here ///
  Tle493dMagnetic3DSensor.begin();
  delay(50);
  Tle493dMagnetic3DSensor.enableTemp();
  delay(50);
  Tle493dMagnetic3DSensor.updateData();

    Xm = Tle493dMagnetic3DSensor.getX();
    Ym = Tle493dMagnetic3DSensor.getY();
    Zm = Tle493dMagnetic3DSensor.getZ();
 // angleD = 180+(57.3*Tle493dMagnetic3DSensor.getAzimuth());
 // offset for level
    angleD = (57.3*Tle493dMagnetic3DSensor.getAzimuth())-90; 
    angleR = abs(angleD/57.3);

    digitalWrite(TRIGpin,LOW);
    delayMicroseconds(5);
    digitalWrite(TRIGpin,HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGpin,LOW);
   
    long duration_cm = pulseIn(ECHOpin,HIGH);
    adjacent = offset+ (duration_cm/58);
    
    
   height = tan(angleR)*adjacent;
   //height = tan(angleR)*200; 
  char VAL[4];
  //  Data Optained From Mag ///// 


   SPI.begin();
   SPI.setClockDivider(SPI_CLOCK_DIV4);
   SPI.setBitOrder(MSBFIRST);
   SPI.setDataMode(SPI_MODE0);
  // Set Active Area on Disp with Orientation and Color //
  // more RAM allows more disp area to be used at once
   paint.SetWidth(200);
  paint.SetHeight(200);
  paint.SetRotate(ROTATE_270);
  paint.Clear(UNCOLORED);
  
  paint.DrawVerticalLine (110,120,50,COLORED);
  paint.DrawStringAt(120,140, "Height", &Font16, COLORED);
  paint.DrawHorizontalLine (30,170,80,COLORED);
  paint.DrawStringAt(40,180, "Adjacent", &Font16, COLORED);
  
  paint.DrawLine (30,170,110,120,COLORED);
  
  // itoa int to string conv
  
  paint.DrawStringAt(5,90, "Height", &Font20, COLORED);
  paint.DrawStringAt(130,90, itoa(height,VAL,10), &Font24, COLORED);
  
  paint.DrawStringAt(5,65, "Angle", &Font20, COLORED);
  paint.DrawStringAt(130,65, itoa(abs(angleD+180),VAL,10), &Font24, COLORED);

  paint.DrawStringAt(5,40, "Adjacent", &Font20, COLORED);
  paint.DrawStringAt(130,40, itoa(adjacent,VAL,10), &Font24, COLORED);

  
  paint.DrawStringAt(10,10, "Units in cm & deg", &Font16, COLORED);

////////////////////////////////////////////////////////////////////////////////
   epd.SetFrameMemory(paint.GetImage(), 0, 0, paint.GetWidth(), paint.GetHeight());
   epd.DisplayFrame();
////////////////////////////////////////////////////////////////////////////////

   delay(200);

} // loop ends here //