![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 4:72c26c07c251
- Parent:
- 2:9c83de2d33ca
- Child:
- 5:f3cbcb294ba9
--- a/main.cpp Sat Sep 16 17:18:49 2017 +0000 +++ b/main.cpp Mon Nov 20 00:43:51 2017 +0000 @@ -17,14 +17,14 @@ 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 ); +//AsyncSerial bt( BLUETOOTH_TX, BLUETOOTH_RX ); //DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX ); -LocalFileSystem file_local( FILESYSTEM_PATH ); +LocalFileSystem file_local( FILESYSTEM_PATH ); void beginMotor(); -//void controlMotor( int16_t left, int16_t right ); +void controlMotor( int16_t left, int16_t right ); void sensing_and_control(); -void receive_command(); +//void receive_command(); uint8_t cnt = 0; @@ -45,11 +45,14 @@ 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.K_trans_vel ); + fscanf( fp, "%f\r\n", &control.C_max_angle ); fclose( fp ); + // coefficient control.K_angle /= MPU9250G_DEG2RAD; control.K_angle_vel /= MPU9250G_DEG2RAD; + control.C_max_angle /= MPU9250G_DEG2RAD; // initialize CAN can_driver.frequency( 500000 ); @@ -59,8 +62,11 @@ imu.setAccelRange( BITS_FS_16G ); imu.setGyroRange( BITS_FS_2000DPS ); + // initialize UART for wireless + /* bt.baud( 115200 ); bt.format( 8, RawSerial::None, 1 ); + */ // initialize player /* @@ -95,8 +101,6 @@ msg.data[1] = 0x06; can_driver.write( msg ); - wait( 0.005f ); - msg.format = CANStandard; msg.type = CANData; msg.id = 0x410; @@ -126,8 +130,6 @@ msg.data[7] = 0x00; can_driver.write( msg ); - wait( 0.005f ); - right = right; msg.format = CANStandard; msg.type = CANData; @@ -150,7 +152,7 @@ //LPF variables int16_t com = 0; int16_t comL = 0; - int16_t comR = 0; + int16_t comR = 0; static float angle_int = 0.0f; static float angle_adj = 0.0f; static float angle_adj_t = 0.0f; @@ -176,7 +178,7 @@ 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 - + // update state posture.angle = angle_int + angle_adj; posture.angle_vel = sensor.gyro[1]; @@ -211,7 +213,7 @@ comR -= control.K_rot_vel * pad.x; } - if( posture.angle < MAX_ANGLE && posture.angle > -MAX_ANGLE ){ + if( posture.angle < control.C_max_angle && posture.angle > -control.C_max_angle ){ controlMotor( comL, comR ); }else{ controlMotor( 0, 0 ); @@ -222,6 +224,8 @@ return; } +// receive command function +/* void receive_command(){ uint8_t c = 0; if( bt.readable() ){ @@ -253,3 +257,4 @@ } return; } +*/ \ No newline at end of file