File content as of revision 6:a5f674c2f262:

#include "mbed.h"
#include "math.h"
#include "defines.h"

#define FILESYSTEM_PATH     "local"
#define FILESYSTEM_GAIN     "/local/gain.txt"

#define K_ANGLE     (1000.0f)
#define K_ANGLE_VEL (5.0f)

STRUCTSENSOR         sensor_vehicle;
STRUCTSENSOR         sensor_interface;

STRUCTPOSTURE        posture;
STRUCTCONRTOLPARAM   control;

STCOMPFILTER         cf_vehicle;
STCOMPFILTER         cf_interface;

// class instance
Ticker          tic_sen_ctrl;
CAN             can_driver( CAN_RX, CAN_TX );
MPU9250         imu_vehicle( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK );
MPU9250         imu_interface( MPU9250_SDA, MPU9250_SCL );
//DFPlayerMini    player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
#ifdef USE_FILESYSTEM
    LocalFileSystem file_local( FILESYSTEM_PATH );
#endif

void beginMotor();
void controlMotor( int16_t left, int16_t right );
void sensing_and_control();
//void receive_command();

int main() {
    uint8_t isLoop = 0x01;
    
    #ifdef USE_SERIAL_DEBUG
        usb_serial.baud( DEBUG_BAUDRATE );
        usb_serial.format( 8, RawSerial::None, 1 );        
        usb_serial.printf( "USB Serial Debug Enable\n" );
    #endif
    
    // initialize control gain
    #ifdef USE_FILESYSTEM    
        FILE*   fp;    
        fp  =   fopen( FILESYSTEM_GAIN, "r" );
        fscanf( fp, "%f\r\n", &control.K_angle     );
        fscanf( fp, "%f\r\n", &control.K_angle_vel );
        fscanf( fp, "%f\r\n", &control.K_wheel     );
        fscanf( fp, "%f\r\n", &control.K_wheel_vel );
        fscanf( fp, "%f\r\n", &control.K_rot_vel   );
        fscanf( fp, "%f\r\n", &control.K_trans_vel );
        fscanf( fp, "%f\r\n", &control.C_max_angle );
        fclose( fp );
    #else
        control.K_angle     =   0.0f;
        control.K_angle_vel =   0.0f;
        control.K_wheel     =   0.0f;
        control.K_wheel_vel =   0.0f;
        control.K_rot_vel   =   0.0f;
        control.K_trans_vel =   0.0f;
        control.C_max_angle =   0.0f;
    #endif
    
    // filter
    init_comp_filter( &cf_vehicle,   3.0f );
    init_comp_filter( &cf_interface, 3.0f );
    
    // coefficient
    control.K_angle     /= MPU9250G_DEG2RAD;
    control.K_angle_vel /= MPU9250G_DEG2RAD;
    control.C_max_angle /= MPU9250G_DEG2RAD;
    
    // initialize CAN
    can_driver.frequency( 500000 );
    
    // initialize sensor
    imu_vehicle.begin();
    imu_vehicle.setAccelRange( BITS_FS_16G );
    imu_vehicle.setGyroRange(  BITS_FS_2000DPS );
    
//    imu_interface.begin();
//    imu_interface.setAccelRange( BITS_FS_16G );
//    imu_interface.setGyroRange(  BITS_FS_2000DPS );
    
    // initialize MP3 player
    /*
    player.begin();
    player.volumeSet( 5 );
    player.playNumber( 1 );
    */
    
    // wait motor driver ON
    wait( 0.1f );
    
    // move driver to operation
    beginMotor();
    
    // start ticker interrupt
    tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC );

    // main loop
    while( isLoop ){
    }
    return 0;
}

void beginMotor(){
    CANMessage msg;
    
    msg.format  =   CANStandard;
    msg.type    =   CANData;    
    msg.id      =   0x410;
    msg.len     =   2;
    msg.data[0] =   DEVID_LEFT;
    msg.data[1] =   0x06;
    can_driver.write( msg );
    
    msg.format  =   CANStandard;
    msg.type    =   CANData;    
    msg.id      =   0x410;
    msg.len     =   2;
    msg.data[0] =   DEVID_RIGHT;
    msg.data[1] =   0x06;
    can_driver.write( msg );    
    
    return;
}

void controlMotor( int16_t left, int16_t right ){
    CANMessage msg;
    
    left = -left;
    msg.format  =   CANStandard;
    msg.type    =   CANData;    
    msg.id      =   0x420;
    msg.len     =   8;
    msg.data[0] =   DEVID_LEFT;
    msg.data[1] =   0x06;
    msg.data[2] =   0x00;
    msg.data[3] =   0x00;
    msg.data[4] =   ( left >> 8 ) & 0xFF;
    msg.data[5] =   ( left >> 0 ) & 0xFF;
    msg.data[6] =   0x00;
    msg.data[7] =   0x00;
    can_driver.write( msg );
    
    right = right;
    msg.format  =   CANStandard;
    msg.type    =   CANData;    
    msg.id      =   0x420;
    msg.len     =   8;
    msg.data[0] =   DEVID_RIGHT;
    msg.data[1] =   0x06;
    msg.data[2] =   0x00;
    msg.data[3] =   0x00;
    msg.data[4] =   ( right >> 8 ) & 0xFF;
    msg.data[5] =   ( right >> 0 ) & 0xFF;
    msg.data[6] =   0x00;
    msg.data[7] =   0x00;    
    can_driver.write( msg );
    
    return;
}

void sensing_and_control(){
    //LPF variables
    int16_t com  = 0;
    int16_t comL = 0;
    int16_t comR = 0;
    float if_angle      =   0.0f;
    float if_angle_vel  =   0.0f;
    
static float   angle_int     =   0.0f;
static float   angle_adj     =   0.0f;
static float   angle_adj_t   =   0.0f;
    
    
    // read sensor ( vehicle )
    imu_vehicle.read6Axis(
        &sensor_vehicle.acc[0],  &sensor_vehicle.acc[1],  &sensor_vehicle.acc[2], 
        &sensor_vehicle.gyro[0], &sensor_vehicle.gyro[1], &sensor_vehicle.gyro[2]
    );
    
    // read sensor ( interface )
    /*
    imu_interface.read6Axis(
        &sensor_interface.acc[0],  &sensor_interface.acc[1],  &sensor_interface.acc[2], 
        &sensor_interface.gyro[0], &sensor_interface.gyro[1], &sensor_interface.gyro[2]
    );
    */
    
    #ifdef USE_SERIAL_DEBUG
    /*
        usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n", 
            sensor_vehicle.acc[0],  sensor_vehicle.acc[1],  sensor_vehicle.acc[2], 
            sensor_vehicle.gyro[0], sensor_vehicle.gyro[1], sensor_vehicle.gyro[2]        
        );
    */
        /*
        usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n", 
            sensor_interface.acc[0],  sensor_interface.acc[1],  sensor_interface.acc[2], 
            sensor_interface.gyro[0], sensor_interface.gyro[1], sensor_interface.gyro[2]        
        );
        */
        usb_serial.printf( "\n" );         
    #endif
    
    // unit convert ( deg -> rad )
    sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD;
    sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD;
    sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD;
    
    /*
    sensor_interface.gyro[0] *= MPU9250G_DEG2RAD;
    sensor_interface.gyro[1] *= MPU9250G_DEG2RAD;
    sensor_interface.gyro[2] *= MPU9250G_DEG2RAD;
    */

    // filter
    /*
    proc_comp_filter( &cf_vehicle, PROCESS_INTERVAL_SEC, 
        -( sensor_vehicle.acc[0] / sensor_vehicle.acc[2] ), sensor_vehicle.gyro[1],
        &posture.angle, &posture.angle_vel );
    */
        
    angle_adj_t = angle_adj;

    //angle estimation using LPF
    angle_int   +=  sensor_vehicle.gyro[1] * PROCESS_INTERVAL_SEC;                                             //integral gyro
    angle_adj   =  atan2( -sensor_vehicle.acc[0] , sensor_vehicle.acc[2] ) - angle_int;                         
    angle_adj   =   angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f;  //calc correction via LPF
    //calc angle
    posture.angle     =   angle_int + angle_adj;
    posture.angle_vel =   sensor_vehicle.gyro[1];

    /*        
    proc_comp_filter( &cf_interface, PROCESS_INTERVAL_SEC, 
        -( sensor_interface.acc[0] / sensor_interface.acc[2] ), sensor_interface.gyro[1],
        &if_angle, &if_angle_vel );
    */
        
    #ifdef USE_SERIAL_DEBUG
        usb_serial.printf( "%f\t %f\n",
            posture.angle     / MPU9250G_DEG2RAD, 
            posture.angle_vel / MPU9250G_DEG2RAD
        );
    #endif
    
    // control motor
    com  =  0;
    com +=  control.K_angle      * posture.angle;
    com +=  control.K_angle_vel  * posture.angle_vel;

    comL   = com;
    comL  += control.K_wheel     * posture.wheel[0];
    comL  += control.K_wheel_vel * posture.wheel[0];

    comR   = com;
    comR  += control.K_wheel     * posture.wheel[1];
    comR  += control.K_wheel_vel * posture.wheel[1];
    
    if( posture.angle < control.C_max_angle && posture.angle > -control.C_max_angle ){
        controlMotor( comL, comR );
    }else{
        controlMotor( 0, 0 );        
    }
    
    return;
}