NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

behaviour.cpp

Committer:
mr_wang
Date:
2018-05-25
Revision:
4:99891561a38b

File content as of revision 4:99891561a38b:

#include "mbed.h"
#include "LSM6DSLSensor.h"
#include "accgyro.h"

DigitalOut led4(LED4);
int32_t BEHAVIOUR_gyro_x;
int32_t BEHAVIOUR_gyro_y;
int32_t BEHAVIOUR_gyro_z;
int32_t BEHAVIOUR_acc_x;
int32_t BEHAVIOUR_acc_y;
int32_t BEHAVIOUR_acc_z;

EventQueue BEHAVIOUR_queue;

void BEHAVIOUR_acc_x_set(int32_t temp)
{
    BEHAVIOUR_acc_x = temp;
}

void BEHAVIOUR_acc_y_set(int32_t temp)
{
    BEHAVIOUR_acc_y = temp;
}

void BEHAVIOUR_acc_z_set(int32_t temp)
{
    BEHAVIOUR_acc_z = temp;
}

int32_t BEHAVIOUR_acc_x_get()
{
    return BEHAVIOUR_acc_x;
}

int32_t BEHAVIOUR_acc_y_get()
{
    return BEHAVIOUR_acc_y;
}

int32_t BEHAVIOUR_acc_z_get()
{
    return BEHAVIOUR_acc_z;
}

void BEHAVIOUR_gyro_x_set(int32_t temp)
{
    BEHAVIOUR_gyro_x = temp;
}

void BEHAVIOUR_gyro_y_set(int32_t temp)
{
    BEHAVIOUR_gyro_y = temp;
}

void BEHAVIOUR_gyro_z_set(int32_t temp)
{
    BEHAVIOUR_gyro_z = temp;
}

int32_t BEHAVIOUR_gyro_x_get()
{
    return BEHAVIOUR_gyro_x;
}

int32_t BEHAVIOUR_gyro_y_get()
{
    return BEHAVIOUR_gyro_y;
}

int32_t BEHAVIOUR_gyro_z_get()
{
    return BEHAVIOUR_gyro_z;
}

void BEHAVIOUR_task()
{
    //printf("GYRO_x:        %6ld\r\n", BEHAVIOUR_gyro_x/1000);
    //printf("GYRO_y:        %6ld\r\n", BEHAVIOUR_gyro_y/2000);
    //printf("GYRO_z:        %6ld\r\n", BEHAVIOUR_gyro_z/2000);
    //printf("ACC_x:        %6ld\r\n", BEHAVIOUR_acc_x);
    //printf("ACC_y:        %6ld\r\n", BEHAVIOUR_acc_y/(160*6));
    //printf("ACC_z:        %6ld\r\n", BEHAVIOUR_acc_z/(160*6));
}

void BEHAVIOUR_init()
{
    BEHAVIOUR_queue.call_every(100, BEHAVIOUR_task);
    BEHAVIOUR_queue.dispatch();    
}