Junjie Wang / Mbed OS NYP_humanoid

A_NYP_humanoid

Dependencies:   LSM6DSL

Fork of A_NYP_humanoid by Junjie Wang

accgyro.cpp@0:0ea84b3cf851, 2018-04-30 (annotated)

Committer:
mr_wang
Date:
Mon Apr 30 09:43:36 2018 +0000
Revision:
0:0ea84b3cf851
NYP_humanoid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mr_wang 0:0ea84b3cf851 1 #include "LSM6DSLSensor.h"
mr_wang 0:0ea84b3cf851 2 #include "accgyro.h"
mr_wang 0:0ea84b3cf851 3 #include "mbed.h"
mr_wang 0:0ea84b3cf851 4 #include "rtos.h"
mr_wang 0:0ea84b3cf851 5
mr_wang 0:0ea84b3cf851 6 //Serial pc(USBTX, USBRX);
mr_wang 0:0ea84b3cf851 7 DigitalOut led1(LED1);
mr_wang 0:0ea84b3cf851 8 int32_t ACCGYRO_gyro_axes[3];
mr_wang 0:0ea84b3cf851 9 int32_t ACCGYRO_acc_axes[3];
mr_wang 0:0ea84b3cf851 10 static DevI2C devI2c(PB_11,PB_10);
mr_wang 0:0ea84b3cf851 11 static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11);
mr_wang 0:0ea84b3cf851 12 EventQueue ACCGYRO_queue;
mr_wang 0:0ea84b3cf851 13 Mutex ACCGYRO_mutex;
mr_wang 0:0ea84b3cf851 14
mr_wang 0:0ea84b3cf851 15 int32_t ACCGYRO_get_gyro_x()
mr_wang 0:0ea84b3cf851 16 {
mr_wang 0:0ea84b3cf851 17 int32_t temp;
mr_wang 0:0ea84b3cf851 18 //ACCGYRO_mutex.lock();
mr_wang 0:0ea84b3cf851 19 temp = ACCGYRO_gyro_axes[0];
mr_wang 0:0ea84b3cf851 20 //ACCGYRO_mutex.unlock();
mr_wang 0:0ea84b3cf851 21 return temp;
mr_wang 0:0ea84b3cf851 22 }
mr_wang 0:0ea84b3cf851 23
mr_wang 0:0ea84b3cf851 24 void ACCGYRO_task()
mr_wang 0:0ea84b3cf851 25 {
mr_wang 0:0ea84b3cf851 26 led1 = !led1;
mr_wang 0:0ea84b3cf851 27 //ACCGYRO_mutex.lock();
mr_wang 0:0ea84b3cf851 28 acc_gyro.get_x_axes(ACCGYRO_gyro_axes);
mr_wang 0:0ea84b3cf851 29 acc_gyro.get_g_axes(ACCGYRO_acc_axes);
mr_wang 0:0ea84b3cf851 30 //ACCGYRO_mutex.unlock();
mr_wang 0:0ea84b3cf851 31 //pc.printf("LSM6DSL [acc/mg]: %6ld\r\n", ACCGYRO_gyro_axes[0]);
mr_wang 0:0ea84b3cf851 32 };
mr_wang 0:0ea84b3cf851 33
mr_wang 0:0ea84b3cf851 34 void ACCGYRO_init()
mr_wang 0:0ea84b3cf851 35 {
mr_wang 0:0ea84b3cf851 36 //pc.printf("Initing LSM6DSL\r\n", ACCGYRO_gyro_axes[0]);
mr_wang 0:0ea84b3cf851 37 acc_gyro.init(NULL);
mr_wang 0:0ea84b3cf851 38 acc_gyro.enable_x();
mr_wang 0:0ea84b3cf851 39 acc_gyro.enable_g();
mr_wang 0:0ea84b3cf851 40 //RtosTimer ACCGYRO_Timer(&ACCGYRO_task,osTimerPeriodic);
mr_wang 0:0ea84b3cf851 41 //ACCGYRO_Timer.start(100);
mr_wang 0:0ea84b3cf851 42 //pc.printf("Init done\r\n");
mr_wang 0:0ea84b3cf851 43 //ACCGYRO_queue.call_every(1000, ACCGYRO_task);
mr_wang 0:0ea84b3cf851 44 //ACCGYRO_queue.dispatch();
mr_wang 0:0ea84b3cf851 45 };