File content as of revision 1:a6cb5f642e69:

#include "mbed.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;
STRUCTPOSTURE        posture;
STRUCTCONRTOLPARAM   control;

// class instance
Ticker          tic_sen_ctrl;
CAN             can_driver( CAN_RX, CAN_TX );
MPU9250         imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK );
DFPlayerMini    player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
LocalFileSystem file_local( FILESYSTEM_PATH );    

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

int main() {
    uint8_t isLoop = 0x01;
    FILE*   fp;
    
    #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
    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 );    
    fclose( fp );
    
    control.K_angle     /= MPU9250G_DEG2RAD;
    control.K_angle_vel /= MPU9250G_DEG2RAD;
    
    // initialize CAN
    can_driver.frequency( 500000 );
    
    // initialize sensor
    imu.begin();
    imu.setAccelRange( BITS_FS_16G );
    imu.setGyroRange(  BITS_FS_2000DPS );
    
    // initialize 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 );
    
    wait( 0.005f );
    
    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 );
    
    wait( 0.005f );
    
    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;
    static float   angle_int     =   0.0f;
    static float   angle_adj     =   0.0f;
    static float   angle_adj_t   =   0.0f;
    
    // read sensor
    imu.read6Axis(
        &sensor.acc[0],  &sensor.acc[1],  &sensor.acc[2], 
        &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2]
    );
    #ifdef USE_SERIAL_DEBUG
        usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n", 
            sensor.acc[0],  sensor.acc[1],  sensor.acc[2], 
            sensor.gyro[0], sensor.gyro[1], sensor.gyro[2]        
        );
    #endif
    
    sensor.gyro[0] *= MPU9250G_DEG2RAD;
    sensor.gyro[1] *= MPU9250G_DEG2RAD;
    sensor.gyro[2] *= MPU9250G_DEG2RAD;

    // filter
    angle_adj_t  =  angle_adj;
    angle_int   +=  sensor.gyro[1] * PROCESS_INTERVAL_SEC;                                      // integral gyro
    angle_adj    =  -( sensor.acc[0] / sensor.acc[2] ) - angle_int;                             // 
    angle_adj    =  angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f;    // calc correction via LPF
    
    // update state
    posture.angle        =   angle_int + angle_adj;
    posture.angle_vec    =   sensor.gyro[1];
    
    #ifdef USE_SERIAL_DEBUG
        usb_serial.printf( "%f\t %f\n", 
            posture.angle / MPU9250G_DEG2RAD, 
            posture.angle_vec / MPU9250G_DEG2RAD        
        );
    #endif    
    
    // control motor
    com = (int16_t)( 
          posture.angle      * control.K_angle 
        + posture.angle_vec  * control.K_angle_vel );
    controlMotor( com, com );
    
    return;
}