Shimon AJISAKA
2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 6:a5f674c2f262
- Parent:
- 5:f3cbcb294ba9
- Child:
- 7:77174a098e6f
--- a/main.cpp Mon Nov 20 00:46:15 2017 +0000
+++ b/main.cpp Fri Apr 20 18:14:15 2018 +0000
@@ -1,4 +1,5 @@
#include "mbed.h"
+#include "math.h"
#include "defines.h"
#define FILESYSTEM_PATH "local"
@@ -7,30 +8,32 @@
#define K_ANGLE (1000.0f)
#define K_ANGLE_VEL (5.0f)
-STRUCTSENSOR sensor;
+STRUCTSENSOR sensor_vehicle;
+STRUCTSENSOR sensor_interface;
+
STRUCTPOSTURE posture;
STRUCTCONRTOLPARAM control;
-STRUCTPAD pad;
+STCOMPFILTER cf_vehicle;
+STCOMPFILTER cf_interface;
// 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 );
+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 );
-LocalFileSystem file_local( FILESYSTEM_PATH );
+#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();
-uint8_t cnt = 0;
-
int main() {
uint8_t isLoop = 0x01;
- FILE* fp;
#ifdef USE_SERIAL_DEBUG
usb_serial.baud( DEBUG_BAUDRATE );
@@ -39,15 +42,30 @@
#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 );
- fscanf( fp, "%f\r\n", &control.C_max_angle );
- fclose( fp );
+ #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;
@@ -58,17 +76,15 @@
can_driver.frequency( 500000 );
// initialize sensor
- imu.begin();
- imu.setAccelRange( BITS_FS_16G );
- imu.setGyroRange( BITS_FS_2000DPS );
+ imu_vehicle.begin();
+ imu_vehicle.setAccelRange( BITS_FS_16G );
+ imu_vehicle.setGyroRange( BITS_FS_2000DPS );
- // initialize UART for wireless
- /*
- bt.baud( 115200 );
- bt.format( 8, RawSerial::None, 1 );
- */
+// imu_interface.begin();
+// imu_interface.setAccelRange( BITS_FS_16G );
+// imu_interface.setGyroRange( BITS_FS_2000DPS );
- // initialize player
+ // initialize MP3 player
/*
player.begin();
player.volumeSet( 5 );
@@ -153,42 +169,82 @@
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;
+ 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
- imu.read6Axis(
- &sensor.acc[0], &sensor.acc[1], &sensor.acc[2],
- &sensor.gyro[0], &sensor.gyro[1], &sensor.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.acc[0], sensor.acc[1], sensor.acc[2],
- sensor.gyro[0], sensor.gyro[1], sensor.gyro[2]
+ 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
- sensor.gyro[0] *= MPU9250G_DEG2RAD;
- sensor.gyro[1] *= MPU9250G_DEG2RAD;
- sensor.gyro[2] *= MPU9250G_DEG2RAD;
+ // 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
- 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
+ /*
+ 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;
- // update state
- posture.angle = angle_int + angle_adj;
- posture.angle_vel = sensor.gyro[1];
-
- if( cnt > 0 ){ cnt--; }
-
+ //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_vec / MPU9250G_DEG2RAD
+ usb_serial.printf( "%f\t %f\n",
+ posture.angle / MPU9250G_DEG2RAD,
+ posture.angle_vel / MPU9250G_DEG2RAD
);
#endif
@@ -200,18 +256,10 @@
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 < control.C_max_angle && posture.angle > -control.C_max_angle ){
controlMotor( comL, comR );
@@ -219,42 +267,5 @@
controlMotor( 0, 0 );
}
-// receive_command();
-
return;
-}
-
-// receive command function
-/*
-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;
-}
-*/
\ No newline at end of file
+}
\ No newline at end of file