2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
main.cpp
- Committer:
- bluefish
- Date:
- 2018-04-20
- Revision:
- 6:a5f674c2f262
- Parent:
- 5:f3cbcb294ba9
- Child:
- 7:77174a098e6f
File content as of revision 6:a5f674c2f262:
#include "mbed.h" #include "math.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_vehicle; STRUCTSENSOR sensor_interface; STRUCTPOSTURE posture; STRUCTCONRTOLPARAM control; STCOMPFILTER cf_vehicle; STCOMPFILTER cf_interface; // class instance Ticker tic_sen_ctrl; CAN can_driver( CAN_RX, CAN_TX ); 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 ); #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(); int main() { uint8_t isLoop = 0x01; #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 #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; control.K_angle_vel /= MPU9250G_DEG2RAD; control.C_max_angle /= MPU9250G_DEG2RAD; // initialize CAN can_driver.frequency( 500000 ); // initialize sensor imu_vehicle.begin(); imu_vehicle.setAccelRange( BITS_FS_16G ); imu_vehicle.setGyroRange( BITS_FS_2000DPS ); // imu_interface.begin(); // imu_interface.setAccelRange( BITS_FS_16G ); // imu_interface.setGyroRange( BITS_FS_2000DPS ); // initialize MP3 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 ); 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 ); 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; 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 ( 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_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 // 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 /* 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; //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_vel / 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]; comR = com; comR += control.K_wheel * posture.wheel[1]; comR += control.K_wheel_vel * posture.wheel[1]; if( posture.angle < control.C_max_angle && posture.angle > -control.C_max_angle ){ controlMotor( comL, comR ); }else{ controlMotor( 0, 0 ); } return; }