2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 2:9c83de2d33ca
- Parent:
- 1:a6cb5f642e69
- Child:
- 4:72c26c07c251
diff -r a6cb5f642e69 -r 9c83de2d33ca main.cpp --- 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; +}