Revision 7:77174a098e6f, committed 2018-05-02

Comitter:

bluefish

Date:

Wed May 02 10:57:10 2018 +0000

Parent:

6:a5f674c2f262

Commit message:

for peshala project

Changed in this revision

--- a/Lib_MPU9250.lib	Fri Apr 20 18:14:15 2018 +0000
+++ b/Lib_MPU9250.lib	Wed May 02 10:57:10 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/bluefish/code/Lib_MPU9250_SPI/#5334e111af53
+https://os.mbed.com/users/bluefish/code/Lib_MPU9250_SPI/#c142d5a352c5

--- a/defines.h	Fri Apr 20 18:14:15 2018 +0000
+++ b/defines.h	Wed May 02 10:57:10 2018 +0000
@@ -8,9 +8,14 @@
 #include "Lib_MPU9250.h"
 #include "filter_func.h"
 
-#define USE_SERIAL_DEBUG
-//#define USE_LED_DEBUG
+// compile option
+//#define USE_SERIAL_DEBUG
+#define USE_LED_DEBUG
 #define USE_FILESYSTEM
+#define USE_FIRST_IMU
+#define USE_SECOND_IMU
+#define USE_MOTOR_CONTROL
+#define USE_STEER_CONTROL
 
 #ifdef USE_SERIAL_DEBUG
     #define DEBUG_BAUDRATE  115200
@@ -58,19 +63,31 @@
 } STRUCTPOSTURE;
 
 typedef struct _STRUCTCONRTOLPARAM{
-  float   K_angle;
-  float   K_angle_vel;
-  float   K_wheel;
-  float   K_wheel_vel;
-  float   K_rot_vel;
-  float   K_trans_vel;
-  float   C_max_angle;
+    float   K_angle;
+    float   K_angle_vel;
+    float   K_wheel;
+    float   K_wheel_vel;
+    float   K_rot_vel;
+    float   K_trans_vel;
+    float   C_max_angle;
+    float   C_offset_angle;
 } STRUCTCONRTOLPARAM;
 
+typedef struct _STRUCTGAMEPAD{
+    float x_raw;
+    float y_raw;
+    float x_filtered;
+    float y_filtered;
+    float C_dead_angle;
+    float C_max_angle;
+} STRUCTGAMEPAD;
+
 extern STRUCTSENSOR         sensor_vehicle;
 extern STRUCTSENSOR         sensor_interface;
 extern STRUCTPOSTURE        posture;
 extern STRUCTCONRTOLPARAM   control;
+extern STRUCTGAMEPAD        pad;
+
 
 //extern Ticker           tic_sen_ctrl;
 //extern CAN              can_driver;

--- a/main.cpp	Fri Apr 20 18:14:15 2018 +0000
+++ b/main.cpp	Wed May 02 10:57:10 2018 +0000
@@ -10,9 +10,9 @@
 
 STRUCTSENSOR         sensor_vehicle;
 STRUCTSENSOR         sensor_interface;
-
 STRUCTPOSTURE        posture;
 STRUCTCONRTOLPARAM   control;
+STRUCTGAMEPAD        pad;
 
 STCOMPFILTER         cf_vehicle;
 STCOMPFILTER         cf_interface;
@@ -23,14 +23,23 @@
 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;
@@ -41,31 +50,87 @@
         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.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;
+        // 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
     
-    // filter
-    init_comp_filter( &cf_vehicle,   3.0f );
-    init_comp_filter( &cf_interface, 3.0f );
+    #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;
@@ -76,13 +141,41 @@
     can_driver.frequency( 500000 );
     
     // initialize sensor
-    imu_vehicle.begin();
-    imu_vehicle.setAccelRange( BITS_FS_16G );
-    imu_vehicle.setGyroRange(  BITS_FS_2000DPS );
+    #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
     
-//    imu_interface.begin();
-//    imu_interface.setAccelRange( BITS_FS_16G );
-//    imu_interface.setGyroRange(  BITS_FS_2000DPS );
+    #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
     /*
@@ -92,7 +185,7 @@
     */
     
     // wait motor driver ON
-    wait( 0.1f );
+    wait( 0.1f );    
     
     // move driver to operation
     beginMotor();
@@ -130,8 +223,10 @@
 
 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;
@@ -142,11 +237,12 @@
     msg.data[3] =   0x00;
     msg.data[4] =   ( left >> 8 ) & 0xFF;
     msg.data[5] =   ( left >> 0 ) & 0xFF;
-    msg.data[6] =   0x00;
-    msg.data[7] =   0x00;
+    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;
@@ -157,8 +253,8 @@
     msg.data[3] =   0x00;
     msg.data[4] =   ( right >> 8 ) & 0xFF;
     msg.data[5] =   ( right >> 0 ) & 0xFF;
-    msg.data[6] =   0x00;
-    msg.data[7] =   0x00;    
+    msg.data[6] =   ( cvd  >> 8 ) & 0xFF;
+    msg.data[7] =   ( cvd  >> 0 ) & 0xFF;
     can_driver.write( msg );
     
     return;
@@ -172,80 +268,59 @@
     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]        
+    #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]
         );
-    */
-        /*
-        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" );         
+        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
     
-    // unit convert ( deg -> rad )
-    sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD;
-    sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD;
-    sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD;
+    #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
     
-    /*
-    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
-        );
+        #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
@@ -261,11 +336,39 @@
     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 );        
-    }
+    #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;
 }
\ No newline at end of file