Inclinometer

const char *msg1="Board to pos    ";
const char *msg2="and press RD0   ";
const char *msg3="Zero G values : ";
const char *msg4="One G values :  ";
const char *msg5="Angles:";
const char *msg6="X=     Y=     Z=    ";

char positionNo=1;

signed int x, y, zz;
signed int zeroG_x, zeroG_y, zeroG_z;
signed int oneG_x, oneG_y, oneG_z;
signed int meas_x, meas_y, meas_z;
char text[32];

void Usart_Write_Text(char* wr_txt) {
  while (*wr_txt)
    Usart_Write(*wr_txt++);
    
  Usart_Write(10);
  Usart_Write(13);
}

// --- Copying strings from ROM to RAM
void strConstCpy(char *dest, const char *source) {
  while(*source)
  *dest++ = *source++ ;
  
  *dest = 0 ;
}

void DoMeasureXYZ() {
  meas_x = Adc_Read(0);   // Measure X_out
  meas_y = Adc_Read(1);   // Measure Y_out
  meas_z = Adc_Read(2);   // Measure Z_out
}

void WriteXYZ(signed int wr_x, signed int wr_y, signed int wr_z) {
  strConstCpy(text,msg6);
  if (wr_x < 0) {
     text[2] = '-';
     wr_x = -wr_x;
     }
  else   
     text[2] = wr_x/1000+48;
     
  text[3] = (wr_x/100)%10+48;
  text[4] = (wr_x/10)%10+48;
  text[5] = wr_x%10+48;


  if (wr_y < 0) {
     text[9] = '-';
     wr_y = -wr_y;
     }
  else   
     text[9] = wr_y/1000+48;
     
  text[10] = (wr_y/100)%10+48;
  text[11] = (wr_y/10)%10+48;
  text[12] = wr_y%10+48;
  
  
  if (wr_z < 0) {
     text[16] = '-';
     wr_z = -wr_z;
     }
  else
     text[16] = wr_z/1000+48;

  text[17] = (wr_z/100)%10+48;
  text[18] = (wr_z/10)%10+48;
  text[19] = wr_z%10+48;
  
  Usart_Write_Text(text);
}

void Calibrate() {

  strConstCpy(text,msg1);         //  1__ Put the Accel board in the position 1 :
  text[13] = positionNo++ + 48;   //      parallel to earth's surface
  Usart_Write_Text(text);         //      Z_axis is vertical with Z label UP
  strConstCpy(text,msg2);         //      to measure the Zero Gravity values for X and Y
  Usart_Write_Text(text);         //      and 1G Z value
  while (PORTD.F0 == 0);          //      Loop until RD0 becomes 1

  DoMeasureXYZ();
  zeroG_x = meas_x;
  zeroG_y = meas_y;
  oneG_z  = meas_z;
  Delay_ms(1000);


  strConstCpy(text,msg1);         //  2__ Put the Accel board in the position 2 :
  text[13] = positionNo++ + 48;   //      X_axis is vertical with X label UP
  Usart_Write_Text(text);
  strConstCpy(text,msg2);         //      to measure the 1G X value
  Usart_Write_Text(text);         //      and Zero Gravity value for Z
  while (PORTD.F0 == 0);          //      Loop until RD0 becomes 1

  DoMeasureXYZ();
  oneG_x = meas_x;
  zeroG_z = meas_z;
  Delay_ms(1000);

  strConstCpy(text,msg1);         //  3__ Put the Accel board in the position 3 :
  text[13] = positionNo++ + 48;   //      Y_axis is vertical with Y label UP
  Usart_Write_Text(text);
  strConstCpy(text,msg2);         //      to measure the 1G Y value
  Usart_Write_Text(text);
  while (PORTD.F0 == 0);          //      Loop until RD0 becomes 1

  DoMeasureXYZ();
  oneG_y = meas_y;

  strConstCpy(text,msg3);         // Display ZeroG values
  Usart_Write_Text(text);
  WriteXYZ(zeroG_x, zeroG_y, zeroG_z);
  Delay_ms(3000);

  strConstCpy(text,msg4);         // Display OneG values
  Usart_Write_Text(text);
  WriteXYZ(oneG_x, oneG_y, oneG_z);
  Delay_ms(3000);
}

void Read_Angles() {
 double div_x, x_rad, div_y, y_rad, div_z, z_rad ;

  DoMeasureXYZ();
  div_x = (float)(meas_x-zeroG_x) / (oneG_x-zeroG_x);  // measured_G / one_G
  x_rad = asin( div_x );                               // x angle in radians
  x = (x_rad*180) / 3.14;                              // x angle in degrees

  div_y = (float)(meas_y-zeroG_y) / (oneG_y-zeroG_y);  // measured_G / one_G
  y_rad = asin( div_y );                               // y angle in radians
  y = (y_rad*180) / 3.14;                              // y angle in degrees

  div_z = (float)(meas_z-zeroG_z) / (oneG_z-zeroG_z);  // measured_G / one_G
  z_rad = asin( div_z );                               // z angle in radians
  zz = (z_rad*180) / 3.14;                             // z angle in degrees

  WriteXYZ(x, y, zz);
}

void main() {
  CMCON  = 7;
  ADCON1 = 0x00;      // configure VDD as Vref, RA0, RA1 and RA2 as analog inputs
  TRISA  = 0b00000111;
  PORTD = 0;          // configure RD0 is digital input
  TRISD = 1;

  Usart_Init(9600);   // Initialize USART module
  Delay_ms(100);
  Calibrate();
  strConstCpy(text,msg5);     // Display Angles
  Usart_Write_Text(text);
              
  //--- main loop
  while(1) {
    Read_Angles();
    Delay_ms(1000);
    }
}