File content as of revision 5:4dbed091ec5a:

#include "mbed.h"
#include "encoder.h"
#include "Mx28.h"
#include "PID.h"
#include "LSM9DS1.h"

//********************* Dynamxiel ******************************
#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 1        // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
#define CCW_LIMIT_ANGLE 4095       // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
#define _PI 3.14159265f
//***************************************************************

Serial uart(USBTX, USBRX);
//Serial uart(D10,D2);            // TX : D10     RX : D2           // blueteeth
DigitalOut LED(A4);            // check if the code is running
DigitalOut led2(A5);            // check if the code is running interrupt
uint8_t led2f;
AnalogIn EMG(PC_4);
DigitalOut test(PC_8);
//AnalogOut test2(PA_5);
// IMU
LSM9DS1 imu(D14, D15);    //SDA SCL
LSM9DS1 imu2(PC_9, D7);   //SDA SCL
void init_IMU();
int16_t Gyro_axis_data[6] = {0};     // X, Y, Z axis
int16_t Acce_axis_data[6] = {0};     // X, Y, Z axis
float Acce_axis_data_f[6] = {0};
float Acce_axis_data_f_old[6] = {0};
float Gyro_axis_data_f[6] = {0};
float Gyro_axis_data_f_old[6] = {0};


// Timer
Ticker timer1;
float ITR_time1 = 6000.0;  // unit：us
float Ts = ITR_time1/1000000;
uint8_t flag;

// EMG
float lpf(float input, float output_old, float frequency);
float emg_value;
float emg_value_f;
float emg_value_f_old;
float Tf = ITR_time1*0.000001f;

// uart_tx
union splitter {
    short j;
    char C[2];
    // C[0] is lowbyte of j, C[1] is highbyte of j
};
char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int i = 0;

// PID
PID motor_pid(5, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
float PIDout = 0.0f;

// Dynamixel
DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
int servo_cmd;
int row_cmd;
int D_angle = 0;
int D_angle_dif = 0;
int D_Angle;
int D_angle_old;
unsigned short d = 0;
// Find Torque
double _angle_difference = 0.0;
double torque_measured = 0.0;
float ks = 2.6393*2;  //spring constant
//int angle_dif = 0;
float torque_ref = 0;
//float friction = 0.0f;
float friction = 0.0f;
float rate = 0.8;
//float friction = 0.0f;
//float check = 0.0f;
float Angle_Dif;
short rotation_;
// function
void init_UART();
void init_TIMER();
void timer1_ITR();
void uart_tx();

void init_DYNAMIXEL();
void D_angle_measure();
void find_torque();

int main()
{
    LED = 1; // darken
    wait_ms(500);
    // initial sensor
    init_SPI_encoder();
    init_encoder();
    init_IMU();
    init_DYNAMIXEL();
    // initial uart
    init_UART();

    wait_ms(500);

    led2 = 1;
//    led2f = 0;
    LED = 0; // lighten

    init_TIMER();

    while(1) {

    }
}

void init_IMU()
{
    imu.begin();
    imu2.begin();
}

void init_DYNAMIXEL()
{
//    dynamixelClass.torqueMode(SERVO_ID, 0);
    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
//    dynamixelClass.setPID(SERVO_ID, 1, 0, 0);
    wait_ms(1);
}

void init_UART()
{
    uart.baud(115200);
}

void init_TIMER()
{
    timer1.attach_us(&timer1_ITR, ITR_time1);
}

void timer1_ITR()
{
    test = 1;
    led2 = !led2;
    angle_measure();
    D_angle_measure();
    find_torque();
    motor_pid.Compute(torque_ref, torque_measured);
    PIDout = motor_pid.output;
    servo_cmd = -PIDout*22.73f;   // 1023/45rpm = 22.73

    if (servo_cmd > 0) {
//                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*22.73f;
        servo_cmd = servo_cmd;
         rotation_ = 0;// 0：Move Left 
        if (servo_cmd >= 1023)
            servo_cmd = 1023;
    } else {
//                servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*22.73f;
//        servo_cmd = -servo_cmd+1024;
        servo_cmd = -servo_cmd;
        rotation_ = 1;// 1：Move Right 
//        if (servo_cmd >= 2047)
//            servo_cmd = 2047;
        if (servo_cmd >= 1023)
            servo_cmd = 1023;
    }

    row_cmd = servo_cmd;

//    if (servo_cmd > 1023) {
//        row_cmd = -(servo_cmd-1023);
//    } else {
//        row_cmd = servo_cmd;
//    }

    // EMG
    emg_value = EMG.read();
    emg_value_f = lpf(emg_value,emg_value_f_old,15);
    emg_value_f_old = emg_value_f;

//            // AnalogOut
//            if (torque_measured*10 > 3.3) {
//                torque_measured = 0.33;
//            } else {
//                test2 = torque_measured*10;
//            }
    // IMU
//            imu.readAccel();
//            imu.readGyro();
//            imu2.readAccel();
//            imu2.readGyro();
    /*
                Acce_axis_data[0] = imu.ax*Acce_gain_x;
                Acce_axis_data[1] = imu.ay*Acce_gain_y;
                Acce_axis_data[2] = imu.az*Acce_gain_z;
                Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
                Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
                Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;

                Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
                Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
                Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
                Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
                Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
                Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;

                for(i=0; i<6; i++) {
                    Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
                    Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
                    Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
                    Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
                }

    */
//    dynamixelClass.torque(SERVO_ID, servo_cmd);
    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023
    wait_ms(1);
    uart_tx();
    flag = 0;
    wait_ms(1);
    test = 0;
}

void uart_tx()
{
    splitter s1;
    splitter s2;
    splitter s3;
    splitter s4;
    splitter s5;
    splitter s6;
    splitter s7;

    s1.j = Angle_Dif;
    s2.j = row_cmd;
//    s2.j = 1;
    s3.j = servo_cmd;
    s4.j = D_Angle;
    s5.j = D_angle;
    s6.j = Angle;
    s7.j = 1;

    T[2] = s1.C[0];
    T[3] = s1.C[1];
    T[4] = s2.C[0];
    T[5] = s2.C[1];
    T[6] = s3.C[0];
    T[7] = s3.C[1];
    T[8] = s4.C[0];
    T[9] = s4.C[1];
    T[10] = s5.C[0];
    T[11] = s5.C[1];
    T[12] = s6.C[0];
    T[13] = s6.C[1];
    T[14] = s7.C[0];
    T[15] = s7.C[1];

    while(1) {
        if (uart.writeable() == 1) {
            uart.putc(T[i]);
            i++;
        }
        if (i >= sizeof(T)) {
            i = 0;
            break;
        }
    }
}

void D_angle_measure()
{
    D_angle = dynamixelClass.readPosition(SERVO_ID);

    if (d == 0) {
        D_Angle = 0;
        D_angle_old = D_angle;
        d++;
    } else {
        D_angle_dif = D_angle - D_angle_old;
        D_Angle = D_Angle + D_angle_dif;
        D_angle_old = D_angle;
    }
}

void find_torque()
{

    Angle_Dif = (-Angle*3+D_Angle);
//    _angle_difference = (Angle*3-D_Angle)/4096.0f*2*_PI;

    _angle_difference = Angle_Dif/4096*2*_PI;
//    if ((_angle_difference < 0) && (D_angle < 0)) {
    if (_angle_difference < -99) {
        _angle_difference = _angle_difference-100.54;
    }
    if (_angle_difference > 99) {
        _angle_difference = _angle_difference-100.54;
    }
    torque_measured = _angle_difference*ks;
//    torque_measured = Angle_Dif;
}

float lpf(float input, float output_old, float frequency)
{
    float output = 0;

    output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);

    return output;
}