main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "defines.h"
00004 
00005 #define FILESYSTEM_PATH     "local"
00006 #define FILESYSTEM_GAIN     "/local/gain.txt"
00007 
00008 #define K_ANGLE     (1000.0f)
00009 #define K_ANGLE_VEL (5.0f)
00010 
00011 STRUCTSENSOR         sensor_vehicle;
00012 STRUCTSENSOR         sensor_interface;
00013 STRUCTPOSTURE        posture;
00014 STRUCTCONRTOLPARAM   control;
00015 STRUCTGAMEPAD        pad;
00016 
00017 STCOMPFILTER         cf_vehicle;
00018 STCOMPFILTER         cf_interface;
00019 
00020 // class instance
00021 Ticker          tic_sen_ctrl;
00022 CAN             can_driver( CAN_RX, CAN_TX );
00023 MPU9250         imu_vehicle( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK );
00024 MPU9250         imu_interface( MPU9250_SDA, MPU9250_SCL );
00025 //DFPlayerMini    player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
00026 
00027 #ifdef USE_FILESYSTEM
00028     LocalFileSystem file_local( FILESYSTEM_PATH );
00029 #endif
00030 
00031 // variables
00032 float C_dead_angle      =   0.0f;
00033 float C_max_angle       =   0.0f;
00034 float C_offset_angle    =   0.0f;
00035 int16_t C_VD            =   0;
00036 
00037 // function prototype
00038 void beginMotor();
00039 void controlMotor( int16_t left, int16_t right );
00040 void sensing_and_control();
00041 //void receive_command();
00042 float convPadJoystick( float input, const float val_deadzone, const float val_max );
00043 
00044 int main() {
00045     uint8_t isLoop = 0x01;
00046     
00047     #ifdef USE_SERIAL_DEBUG
00048         usb_serial.baud( DEBUG_BAUDRATE );
00049         usb_serial.format( 8, RawSerial::None, 1 );        
00050         usb_serial.printf( "USB Serial Debug Enable\n" );
00051     #endif
00052     
00053     #ifdef USE_LED_DEBUG
00054         myled[0] = 0;
00055         myled[1] = 0;
00056         myled[2] = 0;
00057         myled[3] = 0;
00058     #endif
00059     
00060     // initialize control gain
00061     #ifdef USE_FILESYSTEM    
00062         FILE*   fp;    
00063         fp  =   fopen( FILESYSTEM_GAIN, "r" );
00064         // motor control parameter
00065         fscanf( fp, "%d\r\n", &C_VD     );
00066         // control gain
00067         fscanf( fp, "%f\r\n", &control.K_angle     );
00068         fscanf( fp, "%f\r\n", &control.K_angle_vel );
00069         fscanf( fp, "%f\r\n", &control.K_wheel     );
00070         fscanf( fp, "%f\r\n", &control.K_wheel_vel );
00071         fscanf( fp, "%f\r\n", &control.K_rot_vel   );
00072         fscanf( fp, "%f\r\n", &control.K_trans_vel );
00073         // control parameter
00074         fscanf( fp, "%f\r\n", &control.C_max_angle );
00075         fscanf( fp, "%f\r\n", &control.C_offset_angle );        
00076         // pad parameter
00077         fscanf( fp, "%f\r\n", &C_dead_angle );
00078         fscanf( fp, "%f\r\n", &C_max_angle );
00079         fscanf( fp, "%f\r\n", &C_offset_angle );
00080         fclose( fp );
00081     #else
00082         // control gain
00083         C_VD                    =   0;
00084         control.K_angle         =   800.0f;
00085         control.K_angle_vel     =   5.0f;
00086         control.K_wheel         =   0.0f;
00087         control.K_wheel_vel     =   0.0f;
00088         control.K_rot_vel       =   0.0f;
00089         control.K_trans_vel     =   0.0f;
00090         // control parameter
00091         control.C_max_angle     =   15.0f;
00092         control.C_offset_angle  =   0.0f;
00093         // pad parameter
00094         C_dead_angle            =   5.0f;
00095         C_max_angle             =   11.0f;
00096         C_offset_angle          =   0.0f;        
00097         /*
00098         // control gain
00099         control.K_angle         =   800.0f;
00100         control.K_angle_vel     =   5.0f;
00101         control.K_wheel         =   0.0f;
00102         control.K_wheel_vel     =   0.0f;
00103         control.K_rot_vel       =   0.0f;
00104         control.K_trans_vel     =   0.0f;
00105         // control parameter
00106         control.C_max_angle     =   15.0f;
00107         control.C_offset_angle  =   0.0f;        
00108         // pad parameter
00109         C_dead_angle            =   5.0f;
00110         C_max_angle             =   11.0f;
00111         C_offset_angle          =   0.0f;
00112         */
00113     #endif
00114     
00115     #ifdef USE_SERIAL_DEBUG
00116         usb_serial.printf( "MOTOR PARAMETER\n" );
00117         usb_serial.printf( "C_VD :\t%d\n",          C_VD                );    
00118         usb_serial.printf( "CONTROL GAIN\n" );
00119         usb_serial.printf( "K_angle :\t%f\n",       control.K_angle     );
00120         usb_serial.printf( "K_angle_vel :\t%f\n",   control.K_angle_vel );
00121         usb_serial.printf( "K_wheel :\t%f\n",       control.K_wheel     );
00122         usb_serial.printf( "K_wheel_vel :\t%f\n",   control.K_wheel_vel );
00123         usb_serial.printf( "K_rot_vel :\t%f\n",     control.K_rot_vel   );
00124         usb_serial.printf( "K_trans_vel :\t%f\n",   control.K_trans_vel );
00125         usb_serial.printf( "CONTROL PARAMETER\n" );
00126         usb_serial.printf( "C_max_angle :\t%f\n",   control.C_max_angle     );
00127         usb_serial.printf( "C_offset_angle :\t%f\n",   control.C_offset_angle     );
00128         usb_serial.printf( "PAD PARAMETER\n" );
00129         usb_serial.printf( "C_dead_angle :\t%f\n",   C_dead_angle       );
00130         usb_serial.printf( "C_max_angle :\t%f\n",    C_max_angle        );
00131         usb_serial.printf( "C_offset_angle :\t%f\n", C_offset_angle     );
00132         wait( 5.0f );
00133     #endif
00134     
00135     // coefficient
00136     control.K_angle     /= MPU9250G_DEG2RAD;
00137     control.K_angle_vel /= MPU9250G_DEG2RAD;
00138     control.C_max_angle /= MPU9250G_DEG2RAD;
00139     
00140     // initialize CAN
00141     can_driver.frequency( 500000 );
00142     
00143     // initialize sensor
00144     #ifdef USE_FIRST_IMU
00145         imu_vehicle.begin();
00146         imu_vehicle.setAccelRange( BITS_FS_16G );
00147         imu_vehicle.setGyroRange(  BITS_FS_2000DPS );
00148         init_comp_filter( &cf_vehicle,   3.0f );        
00149     #endif
00150 
00151     #ifdef USE_SECOND_IMU    
00152         imu_interface.begin();
00153         imu_interface.setAccelRange( BITS_FS_16G );
00154         imu_interface.setGyroRange(  BITS_FS_2000DPS );
00155         init_comp_filter( &cf_interface, 3.0f );        
00156     #endif
00157     
00158     #ifdef USE_SERIAL_DEBUG
00159         #ifdef USE_FIRST_IMU
00160             usb_serial.printf( "IMU1 Who am I..." );
00161             while( imu_vehicle.getWhoAmI() != 0x73 && imu_vehicle.getWhoAmI() != 0x71  ){ wait(0.1); }
00162             usb_serial.printf( "ok\n" );
00163         #endif
00164     #endif
00165     #ifdef USE_LED_DEBUG
00166         myled[0] = 1;
00167     #endif        
00168     #ifdef USE_SERIAL_DEBUG    
00169         #ifdef USE_SECOND_IMU
00170             usb_serial.printf( "IMU2 Who am I..." );
00171             while( imu_interface.getWhoAmI() != 0x73 && imu_interface.getWhoAmI() != 0x71 ){ wait(0.1); }
00172             usb_serial.printf( "ok\n" );
00173         #endif
00174     #endif
00175     #ifdef USE_LED_DEBUG
00176         myled[1] = 1;
00177     #endif
00178     
00179     
00180     // initialize MP3 player
00181     /*
00182     player.begin();
00183     player.volumeSet( 5 );
00184     player.playNumber( 1 );
00185     */
00186     
00187     // wait motor driver ON
00188     wait( 0.1f );    
00189     
00190     // move driver to operation
00191     beginMotor();
00192     
00193     // start ticker interrupt
00194     tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC );
00195 
00196     // main loop
00197     while( isLoop ){
00198     }
00199     return 0;
00200 }
00201 
00202 void beginMotor(){
00203     CANMessage msg;
00204     
00205     msg.format  =   CANStandard;
00206     msg.type    =   CANData;    
00207     msg.id      =   0x410;
00208     msg.len     =   2;
00209     msg.data[0] =   DEVID_LEFT;
00210     msg.data[1] =   0x06;
00211     can_driver.write( msg );
00212     
00213     msg.format  =   CANStandard;
00214     msg.type    =   CANData;    
00215     msg.id      =   0x410;
00216     msg.len     =   2;
00217     msg.data[0] =   DEVID_RIGHT;
00218     msg.data[1] =   0x06;
00219     can_driver.write( msg );    
00220     
00221     return;
00222 }
00223 
00224 void controlMotor( int16_t left, int16_t right ){
00225     CANMessage msg;
00226     int16_t cvd = 0;
00227         
00228     left = -left;
00229     cvd = (left < 0) ? -C_VD : C_VD;
00230     msg.format  =   CANStandard;
00231     msg.type    =   CANData;    
00232     msg.id      =   0x420;
00233     msg.len     =   8;
00234     msg.data[0] =   DEVID_LEFT;
00235     msg.data[1] =   0x06;
00236     msg.data[2] =   0x00;
00237     msg.data[3] =   0x00;
00238     msg.data[4] =   ( left >> 8 ) & 0xFF;
00239     msg.data[5] =   ( left >> 0 ) & 0xFF;
00240     msg.data[6] =   ( cvd >> 8 ) & 0xFF;
00241     msg.data[7] =   ( cvd >> 0 ) & 0xFF;
00242     can_driver.write( msg );
00243     
00244     right = right;
00245     cvd = (right < 0) ? -C_VD : C_VD;    
00246     msg.format  =   CANStandard;
00247     msg.type    =   CANData;    
00248     msg.id      =   0x420;
00249     msg.len     =   8;
00250     msg.data[0] =   DEVID_RIGHT;
00251     msg.data[1] =   0x06;
00252     msg.data[2] =   0x00;
00253     msg.data[3] =   0x00;
00254     msg.data[4] =   ( right >> 8 ) & 0xFF;
00255     msg.data[5] =   ( right >> 0 ) & 0xFF;
00256     msg.data[6] =   ( cvd  >> 8 ) & 0xFF;
00257     msg.data[7] =   ( cvd  >> 0 ) & 0xFF;
00258     can_driver.write( msg );
00259     
00260     return;
00261 }
00262 
00263 void sensing_and_control(){
00264     //LPF variables
00265     int16_t com  = 0;
00266     int16_t comL = 0;
00267     int16_t comR = 0;
00268     float if_angle      =   0.0f;
00269     float if_angle_vel  =   0.0f;
00270     
00271     // read sensor ( vehicle )
00272     #ifdef USE_FIRST_IMU
00273         imu_vehicle.read6Axis(
00274             &sensor_vehicle.acc[0],  &sensor_vehicle.acc[1],  &sensor_vehicle.acc[2], 
00275             &sensor_vehicle.gyro[0], &sensor_vehicle.gyro[1], &sensor_vehicle.gyro[2]
00276         );
00277         sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD;
00278         sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD;
00279         sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD;
00280         // estimate angle
00281         proc_comp_filter( &cf_vehicle, PROCESS_INTERVAL_SEC, 
00282             -( sensor_vehicle.acc[0] / sensor_vehicle.acc[2] ), sensor_vehicle.gyro[1],
00283             &posture.angle, &posture.angle_vel );
00284         posture.angle -= control.C_offset_angle * MPU9250G_DEG2RAD;
00285     #endif
00286     
00287     #ifdef USE_SECOND_IMU
00288         // read sensor ( interface )    
00289         imu_interface.read6Axis(
00290             &sensor_interface.acc[0],  &sensor_interface.acc[1],  &sensor_interface.acc[2], 
00291             &sensor_interface.gyro[0], &sensor_interface.gyro[1], &sensor_interface.gyro[2]
00292         );
00293         sensor_interface.gyro[0] *= MPU9250G_DEG2RAD;
00294         sensor_interface.gyro[1] *= MPU9250G_DEG2RAD;
00295         sensor_interface.gyro[2] *= MPU9250G_DEG2RAD;    
00296         // estimate angle        
00297         proc_comp_filter( &cf_interface, PROCESS_INTERVAL_SEC, 
00298             ( sensor_interface.acc[1] / sensor_interface.acc[2] ), sensor_interface.gyro[0],
00299             &if_angle, &if_angle_vel );
00300         if_angle     -=  C_offset_angle * MPU9250G_DEG2RAD;
00301         if_angle     /=  MPU9250G_DEG2RAD;
00302         if_angle_vel /=  MPU9250G_DEG2RAD;
00303     #endif
00304     
00305     #ifdef USE_SERIAL_DEBUG
00306         #ifdef USE_FIRST_IMU
00307             /*
00308             usb_serial.printf( "IMU 1:\t %f\t %f\t %f\t %f\t %f\t %f\n", 
00309                 sensor_vehicle.acc[0],  sensor_vehicle.acc[1],  sensor_vehicle.acc[2], 
00310                 sensor_vehicle.gyro[0], sensor_vehicle.gyro[1], sensor_vehicle.gyro[2]        
00311             );
00312             */
00313             usb_serial.printf( "IMU1 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", posture.angle, posture.angle_vel );            
00314         #endif
00315         #ifdef USE_SECOND_IMU
00316             /*
00317             usb_serial.printf( "IMU 2:%f\t %f\t %f\t %f\t %f\t %f\n", 
00318                 sensor_interface.acc[0],  sensor_interface.acc[1],  sensor_interface.acc[2], 
00319                 sensor_interface.gyro[0], sensor_interface.gyro[1], sensor_interface.gyro[2]        
00320             );            
00321             */
00322             usb_serial.printf( "IMU2 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", if_angle,if_angle_vel );            
00323         #endif
00324     #endif
00325     
00326     // control motor
00327     com  =  0;
00328     com +=  control.K_angle      * posture.angle;
00329     com +=  control.K_angle_vel  * posture.angle_vel;
00330 
00331     comL   = com;
00332     comL  += control.K_wheel     * posture.wheel[0];
00333     comL  += control.K_wheel_vel * posture.wheel[0];
00334 
00335     comR   = com;
00336     comR  += control.K_wheel     * posture.wheel[1];
00337     comR  += control.K_wheel_vel * posture.wheel[1];
00338     
00339     #ifdef USE_STEER_CONTROL
00340         float pad_x = 0.0f;
00341         pad_x =  convPadJoystick( if_angle, C_dead_angle, C_max_angle );
00342         comR += pad_x * control.K_rot_vel;
00343         comL -= pad_x * control.K_rot_vel;
00344     #endif
00345     
00346     #ifdef USE_MOTOR_CONTROL
00347         if( posture.angle < control.C_max_angle*MPU9250G_DEG2RAD && posture.angle > -control.C_max_angle*MPU9250G_DEG2RAD ){
00348             controlMotor( comL, comR );
00349         }else{
00350             controlMotor( 0, 0 );
00351         }
00352     #endif
00353     
00354     return;
00355 }
00356 
00357 float convPadJoystick( float input, const float val_deadzone, const float val_max ){
00358     float ret = 0.0f;
00359     float k   = 1.0f;
00360     if( val_deadzone != val_max ){
00361         k = 1.0f / ( val_max - val_deadzone );
00362         if( input < val_deadzone && input >-val_deadzone ){
00363             ret = 0.0f;    
00364         }else if( input >  val_max ){
00365             ret = 1.0f;
00366         }else if( input < -val_max ){
00367             ret = -1.0f;
00368         }else{
00369             ret  = (input > 0.0f) ? (input - val_deadzone) : (input + val_deadzone);
00370             ret *= k;
00371         }
00372     }
00373     return ret;
00374 }