Diff: main.cpp

Revision:

7:7f9abf427b06

Parent:

6:3bd16fdf0b6e

Child:

8:63eff653d0ba

--- a/main.cpp	Tue Apr 21 07:30:19 2015 +0000
+++ b/main.cpp	Tue Apr 28 14:32:03 2015 +0000
@@ -15,23 +15,55 @@
 Serial debug(P0_2, P0_3);
 
 Ticker infoTicker;
-Vector3 eularAngles;
 
 Timer timer;
 Timer timer2;
 
+void initialize_connection(void);
+ 
+void initialize_connection() 
+    {
+        RN42.putc('$'); // Enter command mode
+        RN42.putc('$'); //
+        RN42.putc('$'); //
+        wait(0.5);
+        RN42.putc('C'); //
+        RN42.putc(','); // Send MAC address
+        RN42.putc('0'); //
+        RN42.putc('0'); //
+        RN42.putc('0'); //
+        RN42.putc('6'); //
+        RN42.putc('6'); //
+        RN42.putc('6'); //
+        RN42.putc('A'); //
+        RN42.putc('0'); //
+        RN42.putc('4'); //
+        RN42.putc('E'); //
+        RN42.putc('1'); //
+        RN42.putc('A'); //
+        wait(0.5);
+        RN42.putc('-'); // Exit command mode
+        RN42.putc('-'); //
+        RN42.putc('-'); //
+        RN42.putc(0x0D); //
+        wait(0.5);
+    }
+
+
 char buffer[200];
 void infoTask(void)
 {
     led1=!led1;
-    printf("%d\r\n",timer2.read_us());
 }
 
+
 int main()
 {
     RN42.baud(9600);
     debug.baud(115200);
 
+   // initialize_connection();
+    
     infoTicker.attach(infoTask,0.2f);
 
     if(imu.isReady()) {
@@ -54,10 +86,9 @@
         if(imu.getFifoCount() >= 48) {
             imu.getFifoBuffer(buffer,  48);
             led2 = !led2;
-            timer2.reset();
         }
 
-        if(timer.read_ms() > 100) {
+        if(timer.read_ms() > 50) {
             timer.reset();
 
             //This is the format of the data in the fifo,
@@ -92,9 +123,17 @@
             */
 
             q1.decode(buffer);
-            eularAngles = q1.getEulerAngles();
-            debug.printf("%f, %f, %f\r\n", eularAngles.x, eularAngles.y, eularAngles.z);
-
+            debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
+            
+            Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
+            Vector3 vector_x_local=q1.rotate(vector_x);
+            Vector3 vector_y_local=q1.rotate(vector_y);
+            Vector3 vector_z_local=q1.rotate(vector_z);
+            Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z);
+            Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared());
+            Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared());
+            float angle=bar_projection_g.dot_product(bar_projection)/bar_projection.length()/bar_projection_g.length();
+            debug.printf("angle: %f \r\n ", angle/PI*180);
 
             //TeaPot Demo Packet for MotionFit SDK
             /*