File content as of revision 7:77174a098e6f:

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

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

// variables
float C_dead_angle      =   0.0f;
float C_max_angle       =   0.0f;
float C_offset_angle    =   0.0f;
int16_t C_VD            =   0;

// function prototype
void beginMotor();
void controlMotor( int16_t left, int16_t right );
void sensing_and_control();
//void receive_command();
float convPadJoystick( float input, const float val_deadzone, const float val_max );

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
    
    #ifdef USE_LED_DEBUG
        myled[0] = 0;
        myled[1] = 0;
        myled[2] = 0;
        myled[3] = 0;
    #endif
    
    // initialize control gain
    #ifdef USE_FILESYSTEM    
        FILE*   fp;    
        fp  =   fopen( FILESYSTEM_GAIN, "r" );
        // motor control parameter
        fscanf( fp, "%d\r\n", &C_VD     );
        // control gain
        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 );
        // control parameter
        fscanf( fp, "%f\r\n", &control.C_max_angle );
        fscanf( fp, "%f\r\n", &control.C_offset_angle );        
        // pad parameter
        fscanf( fp, "%f\r\n", &C_dead_angle );
        fscanf( fp, "%f\r\n", &C_max_angle );
        fscanf( fp, "%f\r\n", &C_offset_angle );
        fclose( fp );
    #else
        // control gain
        C_VD                    =   0;
        control.K_angle         =   800.0f;
        control.K_angle_vel     =   5.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 parameter
        control.C_max_angle     =   15.0f;
        control.C_offset_angle  =   0.0f;
        // pad parameter
        C_dead_angle            =   5.0f;
        C_max_angle             =   11.0f;
        C_offset_angle          =   0.0f;        
        /*
        // control gain
        control.K_angle         =   800.0f;
        control.K_angle_vel     =   5.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 parameter
        control.C_max_angle     =   15.0f;
        control.C_offset_angle  =   0.0f;        
        // pad parameter
        C_dead_angle            =   5.0f;
        C_max_angle             =   11.0f;
        C_offset_angle          =   0.0f;
        */
    #endif
    
    #ifdef USE_SERIAL_DEBUG
        usb_serial.printf( "MOTOR PARAMETER\n" );
        usb_serial.printf( "C_VD :\t%d\n",          C_VD                );    
        usb_serial.printf( "CONTROL GAIN\n" );
        usb_serial.printf( "K_angle :\t%f\n",       control.K_angle     );
        usb_serial.printf( "K_angle_vel :\t%f\n",   control.K_angle_vel );
        usb_serial.printf( "K_wheel :\t%f\n",       control.K_wheel     );
        usb_serial.printf( "K_wheel_vel :\t%f\n",   control.K_wheel_vel );
        usb_serial.printf( "K_rot_vel :\t%f\n",     control.K_rot_vel   );
        usb_serial.printf( "K_trans_vel :\t%f\n",   control.K_trans_vel );
        usb_serial.printf( "CONTROL PARAMETER\n" );
        usb_serial.printf( "C_max_angle :\t%f\n",   control.C_max_angle     );
        usb_serial.printf( "C_offset_angle :\t%f\n",   control.C_offset_angle     );
        usb_serial.printf( "PAD PARAMETER\n" );
        usb_serial.printf( "C_dead_angle :\t%f\n",   C_dead_angle       );
        usb_serial.printf( "C_max_angle :\t%f\n",    C_max_angle        );
        usb_serial.printf( "C_offset_angle :\t%f\n", C_offset_angle     );
        wait( 5.0f );
    #endif
    
    // 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
    #ifdef USE_FIRST_IMU
        imu_vehicle.begin();
        imu_vehicle.setAccelRange( BITS_FS_16G );
        imu_vehicle.setGyroRange(  BITS_FS_2000DPS );
        init_comp_filter( &cf_vehicle,   3.0f );        
    #endif

    #ifdef USE_SECOND_IMU    
        imu_interface.begin();
        imu_interface.setAccelRange( BITS_FS_16G );
        imu_interface.setGyroRange(  BITS_FS_2000DPS );
        init_comp_filter( &cf_interface, 3.0f );        
    #endif
    
    #ifdef USE_SERIAL_DEBUG
        #ifdef USE_FIRST_IMU
            usb_serial.printf( "IMU1 Who am I..." );
            while( imu_vehicle.getWhoAmI() != 0x73 && imu_vehicle.getWhoAmI() != 0x71  ){ wait(0.1); }
            usb_serial.printf( "ok\n" );
        #endif
    #endif
    #ifdef USE_LED_DEBUG
        myled[0] = 1;
    #endif        
    #ifdef USE_SERIAL_DEBUG    
        #ifdef USE_SECOND_IMU
            usb_serial.printf( "IMU2 Who am I..." );
            while( imu_interface.getWhoAmI() != 0x73 && imu_interface.getWhoAmI() != 0x71 ){ wait(0.1); }
            usb_serial.printf( "ok\n" );
        #endif
    #endif
    #ifdef USE_LED_DEBUG
        myled[1] = 1;
    #endif
    
    
    // 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;
    int16_t cvd = 0;
        
    left = -left;
    cvd = (left < 0) ? -C_VD : C_VD;
    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] =   ( cvd >> 8 ) & 0xFF;
    msg.data[7] =   ( cvd >> 0 ) & 0xFF;
    can_driver.write( msg );
    
    right = right;
    cvd = (right < 0) ? -C_VD : C_VD;    
    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] =   ( cvd  >> 8 ) & 0xFF;
    msg.data[7] =   ( cvd  >> 0 ) & 0xFF;
    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;
    
    // read sensor ( vehicle )
    #ifdef USE_FIRST_IMU
        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]
        );
        sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD;
        sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD;
        sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD;
        // estimate angle
        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 );
        posture.angle -= control.C_offset_angle * MPU9250G_DEG2RAD;
    #endif
    
    #ifdef USE_SECOND_IMU
        // 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]
        );
        sensor_interface.gyro[0] *= MPU9250G_DEG2RAD;
        sensor_interface.gyro[1] *= MPU9250G_DEG2RAD;
        sensor_interface.gyro[2] *= MPU9250G_DEG2RAD;    
        // estimate angle        
        proc_comp_filter( &cf_interface, PROCESS_INTERVAL_SEC, 
            ( sensor_interface.acc[1] / sensor_interface.acc[2] ), sensor_interface.gyro[0],
            &if_angle, &if_angle_vel );
        if_angle     -=  C_offset_angle * MPU9250G_DEG2RAD;
        if_angle     /=  MPU9250G_DEG2RAD;
        if_angle_vel /=  MPU9250G_DEG2RAD;
    #endif
    
    #ifdef USE_SERIAL_DEBUG
        #ifdef USE_FIRST_IMU
            /*
            usb_serial.printf( "IMU 1:\t %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( "IMU1 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", posture.angle, posture.angle_vel );            
        #endif
        #ifdef USE_SECOND_IMU
            /*
            usb_serial.printf( "IMU 2:%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( "IMU2 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", if_angle,if_angle_vel );            
        #endif
    #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];
    
    #ifdef USE_STEER_CONTROL
        float pad_x = 0.0f;
        pad_x =  convPadJoystick( if_angle, C_dead_angle, C_max_angle );
        comR += pad_x * control.K_rot_vel;
        comL -= pad_x * control.K_rot_vel;
    #endif
    
    #ifdef USE_MOTOR_CONTROL
        if( posture.angle < control.C_max_angle*MPU9250G_DEG2RAD && posture.angle > -control.C_max_angle*MPU9250G_DEG2RAD ){
            controlMotor( comL, comR );
        }else{
            controlMotor( 0, 0 );
        }
    #endif
    
    return;
}

float convPadJoystick( float input, const float val_deadzone, const float val_max ){
    float ret = 0.0f;
    float k   = 1.0f;
    if( val_deadzone != val_max ){
        k = 1.0f / ( val_max - val_deadzone );
        if( input < val_deadzone && input >-val_deadzone ){
            ret = 0.0f;    
        }else if( input >  val_max ){
            ret = 1.0f;
        }else if( input < -val_max ){
            ret = -1.0f;
        }else{
            ret  = (input > 0.0f) ? (input - val_deadzone) : (input + val_deadzone);
            ret *= k;
        }
    }
    return ret;
}