test_code_for_ahrs

Dependencies:   mbed-rtos mbed

Revision:
0:c06709901293
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 25 23:04:09 2016 +0000
@@ -0,0 +1,58 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "MPU6050.h"
+
+#define gyro_read_address_H (0x1D)
+#define gyro_read_address_L (0x1E)
+
+
+Serial pc(USBTX, USBRX); // tx, rx
+MPU6050 imu;
+//imu(I, PinName scl);
+
+
+
+void print_char(char c = '*')
+{
+    printf("%c", c);
+    fflush(stdout);
+}
+
+DigitalOut led1(LED1);
+
+void print_thread(void const *argument)
+{
+    while (true) {
+        Thread::wait(1000);
+        print_char();
+    }
+}
+
+void read_imu(void const *argumennt) {
+    while (true) {
+        Thread::wait(1000);
+        uint8_t whoami = imu.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+        pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
+    }
+}
+
+int main()
+{
+    printf("\n\n*** RTOS basic example ***\n");
+    
+    Thread thread1(read_imu, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    //Thread thread2(print_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    
+    //int gyro_val = 0;
+    //char data_read[2];
+    
+    while (true) {
+        led1 = !led1;
+        //imu.read(gyro_read_address_H, data_read, 0); // no stop
+        //imu.read(gyro_read_address_L, data_read, 0); // 
+        //gyro_val = gyro_val | (int) data_read[0];
+        //gyro_val = gyro_val | ((int) data_read[0] << 8);
+        //printf("Gyro Value: %d\n", data_read[0]);
+        Thread::wait(500);
+    }
+}
\ No newline at end of file