Diff: main.cpp

Revision:

1:19c7e004b469

Parent:

0:045cab440776

Child:

2:b99449535e77

diff -r 045cab440776 -r 19c7e004b469 main.cpp
--- a/main.cpp	Tue Aug 18 20:17:19 2020 +0000
+++ b/main.cpp	Wed Aug 19 11:15:23 2020 +0000
@@ -1,20 +1,32 @@
 
 #include "mbed.h"
 #include "MMA7660.h"
+
+#define PI 3.14159265
  
  
 MMA7660 MMA(p28, p27);
 DigitalOut connectionLed(LED1);
 
-
+float calculateAngle(float x, float y, float z){
+    float angle = 0;
+    double val = 180 / PI;
+    
+    double bot_part = sqrt( pow((double)y, 2.0) + pow((double)z, 2.0) );
+    double top_part = (double)x;
+    angle = atan(top_part/bot_part);
+    
+    return angle*val;
+}
 int main() { 
 
-if (MMA.testConnection()) //test if accelaretor
+if (MMA.testConnection()) //LED shows connection is ok
 connectionLed = 1;
    
     while(1) {
-        printf ("%f, %f, %f \r", MMA.x(), MMA.z(), MMA.x()); //read x,y,z
-        wait(1);
+        double res = calculateAngle(MMA.x(), MMA.z(), MMA.x());
+        printf("result = %f \r", res);
+        wait(0.5);
               
             }