File content as of revision 5:f3cbcb294ba9:

#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;
STRUCTPAD            pad;


// class instance
Ticker          tic_sen_ctrl;
CAN             can_driver( CAN_RX, CAN_TX );
MPU9250         imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK );
//AsyncSerial     bt( BLUETOOTH_TX, BLUETOOTH_RX );
//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();
//void receive_command();

uint8_t cnt = 0;

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 );
    fscanf( fp, "%f\r\n", &control.C_max_angle );
    fclose( fp );
    
    // 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.begin();
    imu.setAccelRange( BITS_FS_16G );
    imu.setGyroRange(  BITS_FS_2000DPS );
    
    // initialize UART for wireless
    /*
    bt.baud( 115200 );
    bt.format( 8, RawSerial::None, 1 );
    */
    
    // 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 );
    
    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;
    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_vel    =   sensor.gyro[1];
    
    if( cnt > 0 ){ cnt--; }
    
    #ifdef USE_SERIAL_DEBUG
        usb_serial.printf( "%f\t %f\n", 
            posture.angle / MPU9250G_DEG2RAD, 
            posture.angle_vec / 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];
    if( pad.enable && cnt > 0 ){    
      comL  += control.K_trans_vel * pad.y;
      comL  += control.K_rot_vel   * pad.x;
    }

    comR   = com;
    comR  += control.K_wheel     * posture.wheel[1];
    comR  += control.K_wheel_vel * posture.wheel[1];
    if( pad.enable && cnt > 0 ){
      comR  += control.K_trans_vel * pad.y;
      comR  -= control.K_rot_vel   * pad.x;
    }
    
    if( posture.angle < control.C_max_angle && posture.angle > -control.C_max_angle ){
        controlMotor( comL, comR );
    }else{
        controlMotor( 0, 0 );        
    }
    
//    receive_command();
    
    return;
}

// receive command function
/* 
void receive_command(){
    uint8_t c = 0;
    if( bt.readable() ){
        c = bt.getc();
        
        switch( c ){
            case 'L':
                pad.x = -1.0f;
                cnt = 100;
                break;
            case 'R':
                pad.x =  1.0f;
                cnt = 100;                
                break;
            case 'S':
                pad.x =  0.0f;
                pad.y =  0.0f;
                cnt = 100;                
                break;
            case 'I':
                pad.enable = 0x01;
                cnt = 100;                
                break;            
            case 'O': 
                pad.enable = 0x00;
                cnt = 100;                
                break;
        }
    }
    return;    
}
*/