File content as of revision 0:bf9bf4b7625f:

#include "mbed.h"
#include "math.h"
#include "LSM9DS0.h"
#include "Mx28.h"
#include "Controller.h"
#include "SensorFusion.h"
#include "SystemConstant.h"

// Dynamixel **************************************************************************************************************************************************
#define Steering_SERVO_ID           3
#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
DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
float deg_s = 0.0;
float left_motor_angle = 0.0f;
float right_motor_angle = 0.0f;

// LSM9DS0 *****************************************************************************************************************************************************
// 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.002403878;                 // -9.81 / ( 3317.81 - (-764.05) )
#define Acce_gain_y   -0.002375119;                 // -9.81 / ( 3476.34 - (-676.806))
#define Acce_gain_z   -0.002454702;                 // -9.81 / ( 4423.63 - (285.406) )
#define Magn_gain     0

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

// 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] = {51.38514174, 62.64907136, -20.82209189};//{52.6302,57.3614,-48.6806};
//Gyro offset
//**********************************************
//41.5376344    -26.92864125    103.3949169
//42.53079179   -27.71065494    97.0400782
//43.54056696   -28.63734115    90.77419355
//**********************************************


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] = {-754.5894428, -677.0645161, 413.4472141};//{-818.8824, -714.3789, 326.2993};//{-721.0150,-748.3353,394.9194};
//Acce offset
//**********************************************
//-618.8191593    -639.3255132    4676.384164
//-604.7047898    3395.289345     375.0957967
//4676.57478     -700.4506354   416.9599218
//**********************************************
LSM9DS0 sensor(SPI_MODE, PB_2, PB_1);    // PB_13, PB_14, PB_15

// Useful constants  *******************************************************************************************************************************************
#define T 0.001 //sampling time
#define CNT2STR 1500 // 1500(ms) = 1.5(s)
// mbed function ***********************************************************************************************************************************************
Ticker timer1;
Ticker timer2;

Serial USB(USBTX, USBRX);
Serial BT_Cmd(PC_12, PD_2);
Serial BT_Data(PC_10, PC_11);
Serial MCU(PB_6, PA_10);

// Linear actuator *********************************************************************************************************************************************
PwmOut pwm_l(D7);
PwmOut pwmn_l(D11);

PwmOut pwm_r(D8);
PwmOut pwmn_r(A3);

#define down_duty 1.0f
#define up_duty   0.0f
#define stop_duty 0.5f

// ADC
AnalogIn RightActuator(PC_0);
AnalogIn LeftActuator(PC_1);

// Functions ***************************************************************************************************************************************************
void init_sensor(void);
void read_sensor(void);
void timer1_interrupt(void);
void timer2_interrupt(void);
void uart_send(void);
void separate(void);

// Variables ***************************************************************************************************************************************************
int i = 0;
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
char BTCmd = 0;
char MCU_Data_get = 0;
char MCU_Data_put = 0;
float velocity = 0.0f;
float velocity_old = 0.0f;
float speed = 0.0f;
float VelocityCmd = 0.0f;
float VelocityCmdOld = 0.0f;
float steer_out = 0.0f;
float steer_out_old = 0.0f;
bool start_control = 0;
float roll_sum = 0.0f;
float average = 0.0f;
int temp = 0;
// turning command
int count2straight = 0;
float roll_cmd = 0.0f;
float roll_old = 0.0f;
float roll = 0.0f;

float steer_cmd = 0.0f;
float steer_old = 0.0f;
float steer = 0.0f;
// test tool
bool up = 0;
bool down = 0;
bool tim1 = 0;
int counter = 0;

// Code ********************************************************************************************************************************************************
// main ********************************************************************************************************************************************************
int main()
{
    dynamixelClass.setMode(Steering_SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE);    // set mode to SERVO and set angle limits
    USB.baud(115200);
    BT_Data.baud(57600);
    BT_Cmd.baud(115200);
    MCU.baud(115200);
    
    // actuator pwm initialize
    pwm_l.period_us(50);    
    pwm_r.period_us(50);    
    // timer setting
    timer1.attach_us(&timer1_interrupt, 1000);//4ms interrupt period
    timer2.attach_us(&timer2_interrupt, 3000);//4.098ms interrupt period
    
    // initialize
    sensor.begin();
    start_control = 0;
    steering_angle = 0;
    
    while(1) {
        // command from tablet
        if(BT_Cmd.readable()) {
            BTCmd = BT_Cmd.getc();
            switch(BTCmd) {
                case 's':
                    start_control = 1;
                    roll_cmd = 2.0f/180.0f*pi;
                    count2straight = 0;
                    break;
                case 'p':
                    start_control = 0;
                    steer_out = 0;
                    steer_rad = 0;
                    steer_rad_old = 0;
                    steering_angle = 0.0f;
                    u_1 = 0;
                    u_2 = 0;
                    u_3 = 0;
                    u_d = 0;
                    u = 0;
                    alpha_1 = 0;
                    alpha_2 = 0;
                    roll_err = 0;
                    VelocityCmd = 0;
                    break;
                case 'f':
                    steer_out = 0;
                    break;
                case 'r':
                    steer_out = steer_out + 2;
//                    up = 1;
//                    down = 0;
                    break;
                case 'l':
                    steer_out = steer_out - 2;
//                    up = 0;
//                    down = 1;
                    break;
                case 'm': // command of turning right
                    roll_cmd = 4.0f/180.0f*pi;
                    count2straight = 0;
                    break;
                case 'n': // command of turning left
                    roll_cmd = 0.0f/180.0f*pi;
                    count2straight = 0;
                    break;
                case 'a':
                    VelocityCmd = VelocityCmd + 0.0f;
                    break;
                case 'b':
                    VelocityCmd = VelocityCmd - 0.0f;
                    break;
                case 'c':
                    break;
                case 'h':
                    VelocityCmd = 0;
                    steer_out = 0;
                    break;
                default:
                    break;
            }
            BTCmd = 0;
        }
    }
}// main

// timer1_interrupt *********************************************************************************************************************************************
void timer1_interrupt(void)
{   
    // MCU_UART ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if(VelocityCmd != VelocityCmdOld) {
        if(MCU.writeable()) {
            MCU_Data_put = VelocityCmd / 0.01953125f;
            MCU.putc(MCU_Data_put);
        }
        VelocityCmdOld = VelocityCmd;
    }
    // speed data from another MCU
    if(MCU.readable())
    {
        MCU_Data_get = MCU.getc();
        tim1=!tim1;
    }
    speed = (float)MCU_Data_get * 0.01953125f;
    // velocity check for auxiliary wheels ///////////////////////////////////////////////////////////////////////////////////
    if(speed < 0.3f){
        counter = 0;
        pwm_l.write(down_duty);
        pwm_r.write(down_duty);
    }
    if(speed > 0.3f){
        counter++;
        pwm_l.write(up_duty);
        pwm_r.write(up_duty);
        if(counter >= 16000){
            counter = 16000;
            pwm_l.write(stop_duty);
            pwm_r.write(stop_duty);
        }
    }
   // if(up ==1){
//        pwm_l.write(up_duty);
//        pwm_r.write(up_duty);
//    }
//    if(down ==1){
//        pwm_l.write(down_duty);
//        pwm_r.write(down_duty);
//    }
    
    TIM1->CCER |= 0x4;
    TIM1->CCER |= 0x40;
    velocity = 1;
    // IMU Read ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    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;

    //Centrifugal Acceleration Compensation ////////////////////////////////////////////////////////////////////////////////////
    /*
                                Acx = Ac*sin(e) = YawRate*YawRate*l_rs
                                Acx = Ac*sin(e)*cos(roll_angle) = YawRate*speed*cos(roll_angle)
                                Acx = Ac*sin(e)*sin(roll_angle) = YawRate*speed*sin(roll_angle)
    */
    Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2];
    Ac[1] = (-1.0) * velocity * Gyro_scale[2] * cos(roll_angle);
    Ac[2] = (-1.0) * velocity * Gyro_scale[2] * sin(roll_angle);
    Acce_scale[0] = Acce_scale[0] - Ac[0];
    Acce_scale[1] = Acce_scale[1] - Ac[1];
    Acce_scale[2] = Acce_scale[2] - Ac[2];
    // do sensor fusion /////////////////////////////////////////////////////////////////////////////////////////////////////////
    roll_fusion(Acce_scale[0],Acce_scale[1],Acce_scale[2],Gyro_scale[2],Gyro_scale[0],5);// alpha = 3 represents the best behavior for robot bike
    droll_angle = lpf(Gyro_scale[0], droll_angle_old, 62.8);  // 62.8 = pi * 20
    droll_angle_old = droll_angle;
    droll_angle = droll_angle + 0.5f/180.0f*pi;
    // bias cancelation
//    roll_angle = roll_angle - 2.2f/180.0f*pi;
    // roll angle of turning command ////////////////////////////////////////////////////////////////////////////////////////////
    roll_ref = lpf(roll_cmd, roll_old, 3.1415f);
    roll_old = roll_ref;
    // steering angle of turning command ////////////////////////////////////////////////////////////////////////////////////////
    /*
                                                        -L*g
                                        steer_ss = ---------------  * roll_ss
                                                     V*V*cos(lammda)
    */
    steer_ref = -L*g/velocity/velocity/cos(lammda)*roll_ref;
    // controller ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if(start_control == 0) {
        deg_s = (180.0f + steer_out)*4096.0f/360.0f;
    }
    if(start_control == 1) {
        
        // lpf for velocity, cut-off freq. = 3Hz = 18.85 (rad/s)
        velocity = lpf(velocity, velocity_old, 18.85);
        velocity_old = velocity;
        
        controller(velocity);
        steer_angle(u, velocity);

        //Steer output
        deg_s = (180.0f - steering_angle)*4096.0f/360.0f;
        // count to turn stright
        if(count2straight < CNT2STR)  count2straight++;
        if(count2straight == CNT2STR) roll_cmd = 2.0f/180.0f*pi;
    }
    // send data by UART //////////////////////////////////////////////////////////////////////////////////////////////////////////
    uart_send();
}// timer1_interrupt

// timer1_interrupt *********************************************************************************************************************************************
void timer2_interrupt(void)
{
 dynamixelClass.servo(Steering_SERVO_ID,deg_s,1000);
}// timer2_interrupt

// read_sensor **************************************************************************************************************************************************
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();
}// read_sensor

// uart_send ****************************************************************************************************************************************************
void uart_send(void)
{
    // uart send data
    display[0] = roll_angle/pi*180.0f*100.0f;
    display[1] = droll_angle/pi*180.0f*100.0f;
    display[2] = speed*100;
    display[3] = steering_angle*100;
    display[4] = u_1*100;
    display[5] = u_2*100;

    separate();

    int j = 0;
    int k = 1;
    while(k)    {
        if(BT_Data.writeable()) {
            BT_Data.putc(num[i]);
            i++;
            j++;
        }
        if(j>1) {// send 2 bytes
            k = 0;
            j = 0;
        }
    }
    if(i>13) i = 0;
}// uart_send

//separate  ****************************************************************************************************************************************************
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;
}//separate