Continue to Site
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);
}
}
