2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
main.cpp
- Committer:
- bluefish
- Date:
- 2017-09-16
- Revision:
- 2:9c83de2d33ca
- Parent:
- 1:a6cb5f642e69
- Child:
- 4:72c26c07c251
File content as of revision 2:9c83de2d33ca:
#include "mbed.h" #include "defines.h" #define FILESYSTEM_PATH "local" #define FILESYSTEM_GAIN "/local/gain.txt" #define K_ANGLE (1000.0f) #define K_ANGLE_VEL (5.0f) 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 ); 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; FILE* fp; #ifdef USE_SERIAL_DEBUG usb_serial.baud( DEBUG_BAUDRATE ); usb_serial.format( 8, RawSerial::None, 1 ); usb_serial.printf( "USB Serial Debug Enable\n" ); #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 ); fclose( fp ); control.K_angle /= MPU9250G_DEG2RAD; control.K_angle_vel /= MPU9250G_DEG2RAD; // initialize CAN can_driver.frequency( 500000 ); // initialize sensor imu.begin(); imu.setAccelRange( BITS_FS_16G ); imu.setGyroRange( BITS_FS_2000DPS ); bt.baud( 115200 ); bt.format( 8, RawSerial::None, 1 ); // initialize player /* player.begin(); player.volumeSet( 5 ); player.playNumber( 1 ); */ // wait motor driver ON wait( 0.1f ); // move driver to operation beginMotor(); // start ticker interrupt tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC ); // main loop while( isLoop ){ } return 0; } void beginMotor(){ CANMessage msg; msg.format = CANStandard; msg.type = CANData; msg.id = 0x410; msg.len = 2; msg.data[0] = DEVID_LEFT; msg.data[1] = 0x06; can_driver.write( msg ); wait( 0.005f ); msg.format = CANStandard; msg.type = CANData; msg.id = 0x410; msg.len = 2; msg.data[0] = DEVID_RIGHT; msg.data[1] = 0x06; can_driver.write( msg ); return; } void controlMotor( int16_t left, int16_t right ){ CANMessage msg; left = -left; msg.format = CANStandard; msg.type = CANData; msg.id = 0x420; msg.len = 8; msg.data[0] = DEVID_LEFT; msg.data[1] = 0x06; msg.data[2] = 0x00; msg.data[3] = 0x00; msg.data[4] = ( left >> 8 ) & 0xFF; msg.data[5] = ( left >> 0 ) & 0xFF; msg.data[6] = 0x00; msg.data[7] = 0x00; can_driver.write( msg ); wait( 0.005f ); right = right; msg.format = CANStandard; msg.type = CANData; msg.id = 0x420; msg.len = 8; msg.data[0] = DEVID_RIGHT; msg.data[1] = 0x06; msg.data[2] = 0x00; msg.data[3] = 0x00; msg.data[4] = ( right >> 8 ) & 0xFF; msg.data[5] = ( right >> 0 ) & 0xFF; msg.data[6] = 0x00; msg.data[7] = 0x00; can_driver.write( msg ); return; } void sensing_and_control(){ //LPF variables 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; // read sensor imu.read6Axis( &sensor.acc[0], &sensor.acc[1], &sensor.acc[2], &sensor.gyro[0], &sensor.gyro[1], &sensor.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] ); #endif sensor.gyro[0] *= MPU9250G_DEG2RAD; sensor.gyro[1] *= MPU9250G_DEG2RAD; sensor.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 // update state posture.angle = angle_int + angle_adj; 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 // control motor 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; }