Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

accgyro.cpp@5:1faeeab28bd1, 2018-06-05 (annotated)

Committer:
mr_wang
Date:
Tue Jun 05 09:34:33 2018 +0000
Revision:
5:1faeeab28bd1
Parent:
4:99891561a38b 
NYP!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mr_wang 4:99891561a38b 1 #include "LSM6DSLSensor.h"
mr_wang 0:0ea84b3cf851 2 #include "accgyro.h"
mr_wang 0:0ea84b3cf851 3 #include "mbed.h"
mr_wang 4:99891561a38b 4 #include "behaviour.h"
mr_wang 0:0ea84b3cf851 5
mr_wang 4:99891561a38b 6 int i;
mr_wang 0:0ea84b3cf851 7 int32_t ACCGYRO_gyro_axes[3];
mr_wang 0:0ea84b3cf851 8 int32_t ACCGYRO_acc_axes[3];
mr_wang 4:99891561a38b 9 int32_t gyro_x[5];
mr_wang 4:99891561a38b 10 int32_t GYRO_Xf = 0;
mr_wang 4:99891561a38b 11 int32_t GYRO_Yf = 0;
mr_wang 4:99891561a38b 12 int32_t GYRO_Zf = 0;
mr_wang 4:99891561a38b 13 int32_t GYRO_Xfold;
mr_wang 4:99891561a38b 14 int32_t GYRO_Yfold;
mr_wang 4:99891561a38b 15 int32_t GYRO_Zfold;
mr_wang 4:99891561a38b 16 float GYRO_X_D;
mr_wang 4:99891561a38b 17 float GYRO_Y_D;
mr_wang 4:99891561a38b 18 float GYRO_Z_D;
mr_wang 4:99891561a38b 19 float GYRO_X_Dold;
mr_wang 4:99891561a38b 20 float GYRO_Y_Dold;
mr_wang 4:99891561a38b 21 float GYRO_Z_Dold;
mr_wang 4:99891561a38b 22 float GYRO_X_Time;
mr_wang 4:99891561a38b 23 float GYRO_Y_Time;
mr_wang 4:99891561a38b 24 float GYRO_Z_Time;
mr_wang 4:99891561a38b 25
mr_wang 0:0ea84b3cf851 26 static DevI2C devI2c(PB_11,PB_10);
mr_wang 0:0ea84b3cf851 27 static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11);
mr_wang 4:99891561a38b 28
mr_wang 4:99891561a38b 29 Timer t;
mr_wang 4:99891561a38b 30 Ticker getdata;
mr_wang 0:0ea84b3cf851 31 EventQueue ACCGYRO_queue;
mr_wang 4:99891561a38b 32 extern EventQueue BEHAVIOUR_queue;
mr_wang 4:99891561a38b 33
ha731548874 2:7d574b1ab3cd 34 //Mutex ACCGYRO_mutex;
mr_wang 0:0ea84b3cf851 35 int32_t ACCGYRO_get_gyro_x()
mr_wang 0:0ea84b3cf851 36 {
mr_wang 0:0ea84b3cf851 37 int32_t temp;
mr_wang 0:0ea84b3cf851 38 temp = ACCGYRO_gyro_axes[0];
mr_wang 4:99891561a38b 39 return temp;
mr_wang 4:99891561a38b 40 }
mr_wang 4:99891561a38b 41
mr_wang 4:99891561a38b 42 int32_t ACCGYRO_get_gyro_y()
mr_wang 4:99891561a38b 43 {
mr_wang 4:99891561a38b 44 int32_t temp;
mr_wang 4:99891561a38b 45 temp = ACCGYRO_gyro_axes[1];
mr_wang 0:0ea84b3cf851 46 return temp;
mr_wang 0:0ea84b3cf851 47 }
mr_wang 0:0ea84b3cf851 48
mr_wang 4:99891561a38b 49 int32_t ACCGYRO_get_gyro_z()
mr_wang 4:99891561a38b 50 {
mr_wang 4:99891561a38b 51 int32_t temp;
mr_wang 4:99891561a38b 52 temp = ACCGYRO_gyro_axes[2];
mr_wang 4:99891561a38b 53 return temp;
mr_wang 4:99891561a38b 54 }
mr_wang 4:99891561a38b 55 /*
mr_wang 4:99891561a38b 56 void GYRO_get()
mr_wang 4:99891561a38b 57 {
mr_wang 4:99891561a38b 58
mr_wang 4:99891561a38b 59 for (i=0; i<5; i++)
mr_wang 4:99891561a38b 60 {
mr_wang 4:99891561a38b 61 gyro_x[i] = ACCGYRO_gyro_axes[0];
mr_wang 4:99891561a38b 62 }
mr_wang 4:99891561a38b 63 printf("%61d\n", &i);
mr_wang 4:99891561a38b 64 }
mr_wang 4:99891561a38b 65 */
mr_wang 4:99891561a38b 66
mr_wang 0:0ea84b3cf851 67 void ACCGYRO_task()
mr_wang 0:0ea84b3cf851 68 {
mr_wang 4:99891561a38b 69 //int i;
mr_wang 4:99891561a38b 70 acc_gyro.get_x_axes(ACCGYRO_acc_axes);
mr_wang 4:99891561a38b 71 acc_gyro.get_g_axes(ACCGYRO_gyro_axes);
mr_wang 4:99891561a38b 72 BEHAVIOUR_queue.call(BEHAVIOUR_gyro_x_set,ACCGYRO_gyro_axes[0]);
mr_wang 4:99891561a38b 73 BEHAVIOUR_queue.call(BEHAVIOUR_gyro_y_set,ACCGYRO_gyro_axes[1]);
mr_wang 4:99891561a38b 74 BEHAVIOUR_queue.call(BEHAVIOUR_gyro_z_set,ACCGYRO_gyro_axes[2]);
mr_wang 4:99891561a38b 75 BEHAVIOUR_queue.call(BEHAVIOUR_acc_x_set,ACCGYRO_acc_axes[0]);
mr_wang 4:99891561a38b 76 BEHAVIOUR_queue.call(BEHAVIOUR_acc_y_set,ACCGYRO_acc_axes[1]);
mr_wang 4:99891561a38b 77 BEHAVIOUR_queue.call(BEHAVIOUR_acc_z_set,ACCGYRO_acc_axes[2]);
mr_wang 4:99891561a38b 78 t. start();
mr_wang 4:99891561a38b 79 for (i=0; i<5; i++)
mr_wang 4:99891561a38b 80 {
mr_wang 4:99891561a38b 81 gyro_x[i] = ACCGYRO_gyro_axes[0];
mr_wang 4:99891561a38b 82 }
mr_wang 4:99891561a38b 83 t.stop();
mr_wang 4:99891561a38b 84 GYRO_X_Time = t.read();
mr_wang 4:99891561a38b 85 //printf("The time taken was %f seconds\n", GYRO_X_Time);
mr_wang 4:99891561a38b 86 t.reset();
mr_wang 4:99891561a38b 87 GYRO_Xf = gyro_x[0];
mr_wang 4:99891561a38b 88 GYRO_Xfold = gyro_x[4];
mr_wang 4:99891561a38b 89 GYRO_X_D = (GYRO_Xf + GYRO_Xfold)*0.5 * GYRO_X_Time *1000+ GYRO_X_Dold;
mr_wang 4:99891561a38b 90 printf("%f\r\n", GYRO_X_D);
mr_wang 4:99891561a38b 91 /*
mr_wang 4:99891561a38b 92 if(GYRO_X_D>180.0)
mr_wang 4:99891561a38b 93 GYRO_X_D = GYRO_X_D - 360.0;
mr_wang 4:99891561a38b 94 else
mr_wang 4:99891561a38b 95 if(GYRO_X_D<-180.0)
mr_wang 4:99891561a38b 96 GYRO_X_D = GYRO_X_D + 360.0;
mr_wang 4:99891561a38b 97 */
mr_wang 0:0ea84b3cf851 98 };
mr_wang 0:0ea84b3cf851 99
mr_wang 0:0ea84b3cf851 100 void ACCGYRO_init()
mr_wang 0:0ea84b3cf851 101 {
mr_wang 4:99891561a38b 102 printf("Initing LSM6DSL\r\n");
mr_wang 0:0ea84b3cf851 103 acc_gyro.init(NULL);
mr_wang 0:0ea84b3cf851 104 acc_gyro.enable_x();
mr_wang 0:0ea84b3cf851 105 acc_gyro.enable_g();
mr_wang 4:99891561a38b 106 printf("Init done\r\n");
mr_wang 4:99891561a38b 107 ACCGYRO_queue.call_every(100, ACCGYRO_task);
mr_wang 4:99891561a38b 108 ACCGYRO_queue.dispatch();
mr_wang 0:0ea84b3cf851 109 };