2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Revision 2:9c83de2d33ca, committed 2017-09-16
- Comitter:
- bluefish
- Date:
- Sat Sep 16 17:15:30 2017 +0000
- Parent:
- 1:a6cb5f642e69
- Child:
- 3:7bcf3407e102
- Commit message:
- ?????????????????????????
Changed in this revision
defines.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/defines.h Sat Sep 16 15:09:53 2017 +0000 +++ b/defines.h Sat Sep 16 17:15:30 2017 +0000 @@ -4,7 +4,7 @@ #include "mbed.h" #include "AsyncSerial.hpp" -#include "Lib_DFPlayerMini.h" +//#include "Lib_DFPlayerMini.h" #include "Lib_MPU9250_SPI.h" //#define USE_SERIAL_DEBUG @@ -19,6 +19,8 @@ DigitalOut myled[4] = { LED1, LED2, LED3, LED4 }; #endif +#define MAX_ANGLE ((20.0f) / MPU9250G_DEG2RAD) + #define PROCESS_INTERVAL_SEC (0.01f) #define CAN_RX (p30) @@ -34,6 +36,9 @@ #define DFPLAYER_TX p28 #define DFPLAYER_BUSY p18 +#define BLUETOOTH_RX p27 +#define BLUETOOTH_TX p28 + #define NEOPIXEL_DOUT (p21) #define DEVID_LEFT (0x01) @@ -47,7 +52,7 @@ typedef struct _STRUCTPOSTURE{ float angle; - float angle_vec; + float angle_vel; float wheel[2]; float wheel_vec[2]; } STRUCTPOSTURE; @@ -61,9 +66,16 @@ float K_trans_vel; } STRUCTCONRTOLPARAM; +typedef struct _STRUCTPAD{ + uint8_t enable; // 制御許可(0=制御しない 1=制御する) + float x; // X軸値( -1.0~1.0 0.0で中立 ) + float y; // Y軸値( -1.0~1.0 0.0で中立 ) +} STRUCTPAD; + extern STRUCTSENSOR sensor; extern STRUCTPOSTURE posture; extern STRUCTCONRTOLPARAM control; +extern STRUCTPAD pad; //extern Ticker tic_sen_ctrl; //extern CAN can_driver;
--- a/main.cpp Sat Sep 16 15:09:53 2017 +0000 +++ b/main.cpp Sat Sep 16 17:15:30 2017 +0000 @@ -10,17 +10,23 @@ 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 ); -DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX ); +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; @@ -53,6 +59,9 @@ imu.setAccelRange( BITS_FS_16G ); imu.setGyroRange( BITS_FS_2000DPS ); + bt.baud( 115200 ); + bt.format( 8, RawSerial::None, 1 ); + // initialize player /* player.begin(); @@ -139,7 +148,9 @@ void sensing_and_control(){ //LPF variables - int16_t com = 0; + 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; @@ -168,20 +179,77 @@ // update state posture.angle = angle_int + angle_adj; - posture.angle_vec = sensor.gyro[1]; + 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 + #endif // control motor - com = (int16_t)( - posture.angle * control.K_angle - + posture.angle_vec * control.K_angle_vel ); - controlMotor( com, com ); + 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 < MAX_ANGLE && posture.angle > -MAX_ANGLE ){ + controlMotor( comL, comR ); + }else{ + controlMotor( 0, 0 ); + } + + receive_command(); return; } + +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; +}