2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 6:a5f674c2f262
- Parent:
- 5:f3cbcb294ba9
- Child:
- 7:77174a098e6f
diff -r f3cbcb294ba9 -r a5f674c2f262 main.cpp --- a/main.cpp Mon Nov 20 00:46:15 2017 +0000 +++ b/main.cpp Fri Apr 20 18:14:15 2018 +0000 @@ -1,4 +1,5 @@ #include "mbed.h" +#include "math.h" #include "defines.h" #define FILESYSTEM_PATH "local" @@ -7,30 +8,32 @@ #define K_ANGLE (1000.0f) #define K_ANGLE_VEL (5.0f) -STRUCTSENSOR sensor; +STRUCTSENSOR sensor_vehicle; +STRUCTSENSOR sensor_interface; + STRUCTPOSTURE posture; STRUCTCONRTOLPARAM control; -STRUCTPAD pad; +STCOMPFILTER cf_vehicle; +STCOMPFILTER cf_interface; // 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 ); +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 ); -LocalFileSystem file_local( FILESYSTEM_PATH ); +#ifdef USE_FILESYSTEM + LocalFileSystem file_local( FILESYSTEM_PATH ); +#endif 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 ); @@ -39,15 +42,30 @@ #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 ); + #ifdef USE_FILESYSTEM + FILE* fp; + 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 ); + #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; + #endif + + // filter + init_comp_filter( &cf_vehicle, 3.0f ); + init_comp_filter( &cf_interface, 3.0f ); // coefficient control.K_angle /= MPU9250G_DEG2RAD; @@ -58,17 +76,15 @@ can_driver.frequency( 500000 ); // initialize sensor - imu.begin(); - imu.setAccelRange( BITS_FS_16G ); - imu.setGyroRange( BITS_FS_2000DPS ); + imu_vehicle.begin(); + imu_vehicle.setAccelRange( BITS_FS_16G ); + imu_vehicle.setGyroRange( BITS_FS_2000DPS ); - // initialize UART for wireless - /* - bt.baud( 115200 ); - bt.format( 8, RawSerial::None, 1 ); - */ +// imu_interface.begin(); +// imu_interface.setAccelRange( BITS_FS_16G ); +// imu_interface.setGyroRange( BITS_FS_2000DPS ); - // initialize player + // initialize MP3 player /* player.begin(); player.volumeSet( 5 ); @@ -153,42 +169,82 @@ 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; + 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 - imu.read6Axis( - &sensor.acc[0], &sensor.acc[1], &sensor.acc[2], - &sensor.gyro[0], &sensor.gyro[1], &sensor.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.acc[0], sensor.acc[1], sensor.acc[2], - sensor.gyro[0], sensor.gyro[1], sensor.gyro[2] + 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" ); #endif - sensor.gyro[0] *= MPU9250G_DEG2RAD; - sensor.gyro[1] *= MPU9250G_DEG2RAD; - sensor.gyro[2] *= MPU9250G_DEG2RAD; + // unit convert ( deg -> rad ) + sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD; + sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD; + sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD; + + /* + sensor_interface.gyro[0] *= MPU9250G_DEG2RAD; + sensor_interface.gyro[1] *= MPU9250G_DEG2RAD; + sensor_interface.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 + /* + 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; - // update state - posture.angle = angle_int + angle_adj; - posture.angle_vel = sensor.gyro[1]; - - if( cnt > 0 ){ cnt--; } - + //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_vec / MPU9250G_DEG2RAD + usb_serial.printf( "%f\t %f\n", + posture.angle / MPU9250G_DEG2RAD, + posture.angle_vel / MPU9250G_DEG2RAD ); #endif @@ -200,18 +256,10 @@ 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 ); @@ -219,42 +267,5 @@ 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; -} -*/ \ No newline at end of file +} \ No newline at end of file