File content as of revision 2:bd98f7a4e231:

#include "mbed.h"
#include "LSM9DS0.h"
#include "Mx28.h"
#include <math.h>

// Dynamixel
#define SERVO_ID 0x01               // ID of which we will set Dynamixel too 
#define SERVO_ControlPin A2       // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer.
#define SERVO_SET_Baudrate 1000000  // Baud rate speed which the Dynamixel will be set too (1Mbps)
#define TxPin A0
#define RxPin A1
#define CW_LIMIT_ANGLE 0x001        // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
#define CCW_LIMIT_ANGLE 0xFFF       // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode

#define pi 3.14
#define Ts 1                          // sampling time
#define A 20                         // angle
#define f 0.5                        // Hz
unsigned long tt = 0;
double deg = 0;
int Pos = 0;
int zero = 190;
char mode = 'c';
DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
DigitalIn mybutton(USER_BUTTON);

//
// Interrupt
//Ticker timer1;
// timer 1
#define LOOPTIME  1000                         // 1 ms
unsigned long lastMilli = 0;
// sampling time
float T = 0.001;

Timer t;
DigitalOut tim1(PC_1);
DigitalOut tim2(PC_0);
Serial USB(USBTX, USBRX);            // TX : D10     RX : D2           // serial 1

void init_uart(void);
void separate(void);
void uart_send(void);
//void timer1_interrupt(void);
void change_mode();
float lpf(float input, float output_old, float frequency);

// uart send data
int display[6] = {0,0,0,0,0,0};
char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0};      // char num[0] , num[1] : 2 start byte

void init_sensor(void);
void read_sensor(void);

LSM9DS0 sensor(SPI_MODE, D9, D6);    // SPI_CS1 : D7 , SPI_CS2 : D6

int sensor_flag = 0;                 // sensor initial flag
int sensor_count = 0;

// sensor data
int16_t Gyro_axis_data[3] = {0};     // X, Y, Z axis
int16_t Acce_axis_data[3] = {0};     // X, Y, Z axis
int16_t Magn_axis_data[3] = {0};     // X, Y, Z axis

// sensor gain
#define Gyro_gain_x   0.002422966362920f
#define Gyro_gain_y   0.002422966362920f          
#define Gyro_gain_z   0.002422966362920f            // datasheet : 70 mdeg/digit
#define Acce_gain_x   -0.002404245422390f    // -9.81 / ( 3317.81 - (-764.05) )
#define Acce_gain_y   -0.002344293490135f    // -9.81 / ( 3476.34 - (-676.806))
#define Acce_gain_z   -0.002307390759185f    // -9.81 / ( 4423.63 - (285.406) )
#define Magn_gain     0

// sensor filter correction data
float Gyro_axis_data_f[3];
float Gyro_axis_data_f_old[3];
float Gyro_scale[3];                          // scale = (data - zero) * gain
float Gyro_axis_zero[3] = {50.095,56.249,1.862};       
//Gyro offset   
//**********************************************
//51.33333333   57.76969697  7.508080808
//49.26363636   55.94646465  -4.662626263
//49.68848849   55.03183183  2.74044044

//**********************************************


float Acce_axis_data_f[3];
float Acce_axis_data_f_old[3];
float Acce_scale[3];                          // scale = (data - zero) * gain
float Acce_axis_zero[3] = {-651.6106,-697.9139,218.8919};         
//Acce offset
//**********************************************
//-670.5135135  -669.5905906    4470.447447
//-632.7077077  3486.715716     186.8898899
//3428.671672   -726.2372372    250.8938939

//**********************************************


float Magn_axis_data_f[3];
float Magn_axis_data_f_old[3];
float Magn_scale[3];                          // scale = (data - zero) * gain
float Magn_axis_zero[3] = {0,0,0};      

int main()
{
    dynamixelClass.setMode(SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE);    // set mode to SERVO and set angle limits
//    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
    USB.attach(&change_mode); 
    t.start();
    
    init_uart();
    init_sensor();

    while(1) 
    {
        // timer 1
        if((t.read_us() - lastMilli) >= LOOPTIME)          // 2000 us = 2 ms
        {    
            tim1 = 1;
            lastMilli = t.read_us();

            // sensor initial start
            if(sensor_flag == 0) 
            {
                sensor_count++;

                if(sensor_count >= 50) 
                {
                    sensor_flag  = 1;
                    sensor_count = 0;
                }
            } 
            else 
            {
                read_sensor();
                Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
                Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
                Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);     
                Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
                Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);     
                Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
                
                Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);     
                Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
                Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);     
                Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
                Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);     
                Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
                
                Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
                Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
                Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
                
                Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x;
                Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y;
                Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z;
                switch (mode) {
                    case 'a':
                        deg = zero*4096.0f/360.0f;
                        break;
                    case 'b':
                        deg = (zero + 90)*4096.0f/360.0f;
                        break;
                    case 'c':
                        deg = (zero-A*sin(2*pi*f*tt*0.001*Ts))*4096.0f/360.0f;       // 在正負A度搖
                        tt = tt + 1;
                        if(tt >= 1/(f*0.001*Ts))                              // 2*pi*f*t*0.001*Ts = 2*pi -> t = 1/(f*0.001*T)
                            tt = 0;
                        break;
                }    
                tim2 = 1;
                dynamixelClass.servo(SERVO_ID,deg,0x400);
//                Pos = dynamixelClass.readPosition(SERVO_ID);   
                tim2 = 0;             
                uart_send();
                tim1 = 0;
            }
        }
    }   // while(1) end
}

int i = 0;
void uart_send(void)
{
    // uart send data
    display[0] = Acce_scale[0]*10;
    display[1] = Acce_scale[1]*10;
    display[2] = Acce_scale[2]*10;
    display[3] = Gyro_scale[0]*10;
    display[4] = Gyro_scale[1]*10;
    display[5] = Gyro_scale[2]*10;

    separate();

    int j = 0;
    int k = 1;
    while(k) 
    {
        if(USB.writeable()) 
        {            
            USB.putc(num[i]);
            i++;
            j++;
        }

        if(j>1)                     // send 2 bytes
        {
            k = 0;
            j = 0;
        }
    }

    if(i>13)
        i = 0;
}

void read_sensor(void)
{
    // sensor raw data
    Gyro_axis_data[0] = sensor.readRawGyroX();
    Gyro_axis_data[1] = sensor.readRawGyroY();
    Gyro_axis_data[2] = sensor.readRawGyroZ();

    Acce_axis_data[0] = sensor.readRawAccelX();
    Acce_axis_data[1] = sensor.readRawAccelY();
    Acce_axis_data[2] = sensor.readRawAccelZ();

    Magn_axis_data[0] = sensor.readRawMagX();
    Magn_axis_data[1] = sensor.readRawMagY();
    Magn_axis_data[2] = sensor.readRawMagZ();
}

void init_uart()
{
    USB.baud(57600);      // 設定baudrate
}

void separate(void)
{
    num[2] = display[0] >> 8;           // HSB(8bit)資料存入陣列
    num[3] = display[0] & 0x00ff;       // LSB(8bit)資料存入陣列
    num[4] = display[1] >> 8;
    num[5] = display[1] & 0x00ff;
    num[6] = display[2] >> 8;
    num[7] = display[2] & 0x00ff;
    num[8] = display[3] >> 8;
    num[9] = display[3] & 0x00ff;
    num[10] = display[4] >> 8;
    num[11] = display[4] & 0x00ff;
    num[12] = display[5] >> 8;
    num[13] = display[5] & 0x00ff;
}

void init_sensor(void)
{
    sensor.begin();
    // sensor.begin() setting :
    // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
    //                accel_scale aScl = A_SCALE_8G,
    //                mag_scale mScl = M_SCALE_8GS,
    //                gyro_odr gODR = G_ODR_760_BW_100,
    //                accel_odr aODR = A_ODR_800,
    //                mag_odr mODR = M_ODR_100);
}

float lpf(float input, float output_old, float frequency)
{
    float output = 0;
    
    output = (output_old + frequency*T*input) / (1 + frequency*T);
    
    return output;
}

void change_mode()
{
    mode = USB.getc();
}