Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

behaviour.cpp@4:99891561a38b, 2018-05-25 (annotated)

Committer:
mr_wang
Date:
Fri May 25 09:00:15 2018 +0000
Revision:
4:99891561a38b
NYP_DAD

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mr_wang 4:99891561a38b 1 #include "mbed.h"
mr_wang 4:99891561a38b 2 #include "LSM6DSLSensor.h"
mr_wang 4:99891561a38b 3 #include "accgyro.h"
mr_wang 4:99891561a38b 4
mr_wang 4:99891561a38b 5 DigitalOut led4(LED4);
mr_wang 4:99891561a38b 6 int32_t BEHAVIOUR_gyro_x;
mr_wang 4:99891561a38b 7 int32_t BEHAVIOUR_gyro_y;
mr_wang 4:99891561a38b 8 int32_t BEHAVIOUR_gyro_z;
mr_wang 4:99891561a38b 9 int32_t BEHAVIOUR_acc_x;
mr_wang 4:99891561a38b 10 int32_t BEHAVIOUR_acc_y;
mr_wang 4:99891561a38b 11 int32_t BEHAVIOUR_acc_z;
mr_wang 4:99891561a38b 12
mr_wang 4:99891561a38b 13 EventQueue BEHAVIOUR_queue;
mr_wang 4:99891561a38b 14
mr_wang 4:99891561a38b 15 void BEHAVIOUR_acc_x_set(int32_t temp)
mr_wang 4:99891561a38b 16 {
mr_wang 4:99891561a38b 17 BEHAVIOUR_acc_x = temp;
mr_wang 4:99891561a38b 18 }
mr_wang 4:99891561a38b 19
mr_wang 4:99891561a38b 20 void BEHAVIOUR_acc_y_set(int32_t temp)
mr_wang 4:99891561a38b 21 {
mr_wang 4:99891561a38b 22 BEHAVIOUR_acc_y = temp;
mr_wang 4:99891561a38b 23 }
mr_wang 4:99891561a38b 24
mr_wang 4:99891561a38b 25 void BEHAVIOUR_acc_z_set(int32_t temp)
mr_wang 4:99891561a38b 26 {
mr_wang 4:99891561a38b 27 BEHAVIOUR_acc_z = temp;
mr_wang 4:99891561a38b 28 }
mr_wang 4:99891561a38b 29
mr_wang 4:99891561a38b 30 int32_t BEHAVIOUR_acc_x_get()
mr_wang 4:99891561a38b 31 {
mr_wang 4:99891561a38b 32 return BEHAVIOUR_acc_x;
mr_wang 4:99891561a38b 33 }
mr_wang 4:99891561a38b 34
mr_wang 4:99891561a38b 35 int32_t BEHAVIOUR_acc_y_get()
mr_wang 4:99891561a38b 36 {
mr_wang 4:99891561a38b 37 return BEHAVIOUR_acc_y;
mr_wang 4:99891561a38b 38 }
mr_wang 4:99891561a38b 39
mr_wang 4:99891561a38b 40 int32_t BEHAVIOUR_acc_z_get()
mr_wang 4:99891561a38b 41 {
mr_wang 4:99891561a38b 42 return BEHAVIOUR_acc_z;
mr_wang 4:99891561a38b 43 }
mr_wang 4:99891561a38b 44
mr_wang 4:99891561a38b 45 void BEHAVIOUR_gyro_x_set(int32_t temp)
mr_wang 4:99891561a38b 46 {
mr_wang 4:99891561a38b 47 BEHAVIOUR_gyro_x = temp;
mr_wang 4:99891561a38b 48 }
mr_wang 4:99891561a38b 49
mr_wang 4:99891561a38b 50 void BEHAVIOUR_gyro_y_set(int32_t temp)
mr_wang 4:99891561a38b 51 {
mr_wang 4:99891561a38b 52 BEHAVIOUR_gyro_y = temp;
mr_wang 4:99891561a38b 53 }
mr_wang 4:99891561a38b 54
mr_wang 4:99891561a38b 55 void BEHAVIOUR_gyro_z_set(int32_t temp)
mr_wang 4:99891561a38b 56 {
mr_wang 4:99891561a38b 57 BEHAVIOUR_gyro_z = temp;
mr_wang 4:99891561a38b 58 }
mr_wang 4:99891561a38b 59
mr_wang 4:99891561a38b 60 int32_t BEHAVIOUR_gyro_x_get()
mr_wang 4:99891561a38b 61 {
mr_wang 4:99891561a38b 62 return BEHAVIOUR_gyro_x;
mr_wang 4:99891561a38b 63 }
mr_wang 4:99891561a38b 64
mr_wang 4:99891561a38b 65 int32_t BEHAVIOUR_gyro_y_get()
mr_wang 4:99891561a38b 66 {
mr_wang 4:99891561a38b 67 return BEHAVIOUR_gyro_y;
mr_wang 4:99891561a38b 68 }
mr_wang 4:99891561a38b 69
mr_wang 4:99891561a38b 70 int32_t BEHAVIOUR_gyro_z_get()
mr_wang 4:99891561a38b 71 {
mr_wang 4:99891561a38b 72 return BEHAVIOUR_gyro_z;
mr_wang 4:99891561a38b 73 }
mr_wang 4:99891561a38b 74
mr_wang 4:99891561a38b 75 void BEHAVIOUR_task()
mr_wang 4:99891561a38b 76 {
mr_wang 4:99891561a38b 77 //printf("GYRO_x: %6ld\r\n", BEHAVIOUR_gyro_x/1000);
mr_wang 4:99891561a38b 78 //printf("GYRO_y: %6ld\r\n", BEHAVIOUR_gyro_y/2000);
mr_wang 4:99891561a38b 79 //printf("GYRO_z: %6ld\r\n", BEHAVIOUR_gyro_z/2000);
mr_wang 4:99891561a38b 80 //printf("ACC_x: %6ld\r\n", BEHAVIOUR_acc_x);
mr_wang 4:99891561a38b 81 //printf("ACC_y: %6ld\r\n", BEHAVIOUR_acc_y/(160*6));
mr_wang 4:99891561a38b 82 //printf("ACC_z: %6ld\r\n", BEHAVIOUR_acc_z/(160*6));
mr_wang 4:99891561a38b 83 }
mr_wang 4:99891561a38b 84
mr_wang 4:99891561a38b 85 void BEHAVIOUR_init()
mr_wang 4:99891561a38b 86 {
mr_wang 4:99891561a38b 87 BEHAVIOUR_queue.call_every(100, BEHAVIOUR_task);
mr_wang 4:99891561a38b 88 BEHAVIOUR_queue.dispatch();
mr_wang 4:99891561a38b 89 }