Shimon AJISAKA
2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 7:77174a098e6f
- Parent:
- 6:a5f674c2f262
--- a/main.cpp Fri Apr 20 18:14:15 2018 +0000
+++ b/main.cpp Wed May 02 10:57:10 2018 +0000
@@ -10,9 +10,9 @@
STRUCTSENSOR sensor_vehicle;
STRUCTSENSOR sensor_interface;
-
STRUCTPOSTURE posture;
STRUCTCONRTOLPARAM control;
+STRUCTGAMEPAD pad;
STCOMPFILTER cf_vehicle;
STCOMPFILTER cf_interface;
@@ -23,14 +23,23 @@
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
+// variables
+float C_dead_angle = 0.0f;
+float C_max_angle = 0.0f;
+float C_offset_angle = 0.0f;
+int16_t C_VD = 0;
+
+// function prototype
void beginMotor();
void controlMotor( int16_t left, int16_t right );
void sensing_and_control();
//void receive_command();
+float convPadJoystick( float input, const float val_deadzone, const float val_max );
int main() {
uint8_t isLoop = 0x01;
@@ -41,31 +50,87 @@
usb_serial.printf( "USB Serial Debug Enable\n" );
#endif
+ #ifdef USE_LED_DEBUG
+ myled[0] = 0;
+ myled[1] = 0;
+ myled[2] = 0;
+ myled[3] = 0;
+ #endif
+
// initialize control gain
#ifdef USE_FILESYSTEM
FILE* fp;
fp = fopen( FILESYSTEM_GAIN, "r" );
+ // motor control parameter
+ fscanf( fp, "%d\r\n", &C_VD );
+ // control gain
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 );
+ // control parameter
fscanf( fp, "%f\r\n", &control.C_max_angle );
+ fscanf( fp, "%f\r\n", &control.C_offset_angle );
+ // pad parameter
+ fscanf( fp, "%f\r\n", &C_dead_angle );
+ fscanf( fp, "%f\r\n", &C_max_angle );
+ fscanf( fp, "%f\r\n", &C_offset_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;
+ // control gain
+ C_VD = 0;
+ control.K_angle = 800.0f;
+ control.K_angle_vel = 5.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 parameter
+ control.C_max_angle = 15.0f;
+ control.C_offset_angle = 0.0f;
+ // pad parameter
+ C_dead_angle = 5.0f;
+ C_max_angle = 11.0f;
+ C_offset_angle = 0.0f;
+ /*
+ // control gain
+ control.K_angle = 800.0f;
+ control.K_angle_vel = 5.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 parameter
+ control.C_max_angle = 15.0f;
+ control.C_offset_angle = 0.0f;
+ // pad parameter
+ C_dead_angle = 5.0f;
+ C_max_angle = 11.0f;
+ C_offset_angle = 0.0f;
+ */
#endif
- // filter
- init_comp_filter( &cf_vehicle, 3.0f );
- init_comp_filter( &cf_interface, 3.0f );
+ #ifdef USE_SERIAL_DEBUG
+ usb_serial.printf( "MOTOR PARAMETER\n" );
+ usb_serial.printf( "C_VD :\t%d\n", C_VD );
+ usb_serial.printf( "CONTROL GAIN\n" );
+ usb_serial.printf( "K_angle :\t%f\n", control.K_angle );
+ usb_serial.printf( "K_angle_vel :\t%f\n", control.K_angle_vel );
+ usb_serial.printf( "K_wheel :\t%f\n", control.K_wheel );
+ usb_serial.printf( "K_wheel_vel :\t%f\n", control.K_wheel_vel );
+ usb_serial.printf( "K_rot_vel :\t%f\n", control.K_rot_vel );
+ usb_serial.printf( "K_trans_vel :\t%f\n", control.K_trans_vel );
+ usb_serial.printf( "CONTROL PARAMETER\n" );
+ usb_serial.printf( "C_max_angle :\t%f\n", control.C_max_angle );
+ usb_serial.printf( "C_offset_angle :\t%f\n", control.C_offset_angle );
+ usb_serial.printf( "PAD PARAMETER\n" );
+ usb_serial.printf( "C_dead_angle :\t%f\n", C_dead_angle );
+ usb_serial.printf( "C_max_angle :\t%f\n", C_max_angle );
+ usb_serial.printf( "C_offset_angle :\t%f\n", C_offset_angle );
+ wait( 5.0f );
+ #endif
// coefficient
control.K_angle /= MPU9250G_DEG2RAD;
@@ -76,13 +141,41 @@
can_driver.frequency( 500000 );
// initialize sensor
- imu_vehicle.begin();
- imu_vehicle.setAccelRange( BITS_FS_16G );
- imu_vehicle.setGyroRange( BITS_FS_2000DPS );
+ #ifdef USE_FIRST_IMU
+ imu_vehicle.begin();
+ imu_vehicle.setAccelRange( BITS_FS_16G );
+ imu_vehicle.setGyroRange( BITS_FS_2000DPS );
+ init_comp_filter( &cf_vehicle, 3.0f );
+ #endif
+
+ #ifdef USE_SECOND_IMU
+ imu_interface.begin();
+ imu_interface.setAccelRange( BITS_FS_16G );
+ imu_interface.setGyroRange( BITS_FS_2000DPS );
+ init_comp_filter( &cf_interface, 3.0f );
+ #endif
-// imu_interface.begin();
-// imu_interface.setAccelRange( BITS_FS_16G );
-// imu_interface.setGyroRange( BITS_FS_2000DPS );
+ #ifdef USE_SERIAL_DEBUG
+ #ifdef USE_FIRST_IMU
+ usb_serial.printf( "IMU1 Who am I..." );
+ while( imu_vehicle.getWhoAmI() != 0x73 && imu_vehicle.getWhoAmI() != 0x71 ){ wait(0.1); }
+ usb_serial.printf( "ok\n" );
+ #endif
+ #endif
+ #ifdef USE_LED_DEBUG
+ myled[0] = 1;
+ #endif
+ #ifdef USE_SERIAL_DEBUG
+ #ifdef USE_SECOND_IMU
+ usb_serial.printf( "IMU2 Who am I..." );
+ while( imu_interface.getWhoAmI() != 0x73 && imu_interface.getWhoAmI() != 0x71 ){ wait(0.1); }
+ usb_serial.printf( "ok\n" );
+ #endif
+ #endif
+ #ifdef USE_LED_DEBUG
+ myled[1] = 1;
+ #endif
+
// initialize MP3 player
/*
@@ -92,7 +185,7 @@
*/
// wait motor driver ON
- wait( 0.1f );
+ wait( 0.1f );
// move driver to operation
beginMotor();
@@ -130,8 +223,10 @@
void controlMotor( int16_t left, int16_t right ){
CANMessage msg;
-
+ int16_t cvd = 0;
+
left = -left;
+ cvd = (left < 0) ? -C_VD : C_VD;
msg.format = CANStandard;
msg.type = CANData;
msg.id = 0x420;
@@ -142,11 +237,12 @@
msg.data[3] = 0x00;
msg.data[4] = ( left >> 8 ) & 0xFF;
msg.data[5] = ( left >> 0 ) & 0xFF;
- msg.data[6] = 0x00;
- msg.data[7] = 0x00;
+ msg.data[6] = ( cvd >> 8 ) & 0xFF;
+ msg.data[7] = ( cvd >> 0 ) & 0xFF;
can_driver.write( msg );
right = right;
+ cvd = (right < 0) ? -C_VD : C_VD;
msg.format = CANStandard;
msg.type = CANData;
msg.id = 0x420;
@@ -157,8 +253,8 @@
msg.data[3] = 0x00;
msg.data[4] = ( right >> 8 ) & 0xFF;
msg.data[5] = ( right >> 0 ) & 0xFF;
- msg.data[6] = 0x00;
- msg.data[7] = 0x00;
+ msg.data[6] = ( cvd >> 8 ) & 0xFF;
+ msg.data[7] = ( cvd >> 0 ) & 0xFF;
can_driver.write( msg );
return;
@@ -172,80 +268,59 @@
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]
+ #ifdef USE_FIRST_IMU
+ 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]
);
- */
- /*
- 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" );
+ sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD;
+ sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD;
+ sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD;
+ // estimate angle
+ 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 );
+ posture.angle -= control.C_offset_angle * MPU9250G_DEG2RAD;
#endif
- // unit convert ( deg -> rad )
- sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD;
- sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD;
- sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD;
+ #ifdef USE_SECOND_IMU
+ // 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]
+ );
+ sensor_interface.gyro[0] *= MPU9250G_DEG2RAD;
+ sensor_interface.gyro[1] *= MPU9250G_DEG2RAD;
+ sensor_interface.gyro[2] *= MPU9250G_DEG2RAD;
+ // estimate angle
+ proc_comp_filter( &cf_interface, PROCESS_INTERVAL_SEC,
+ ( sensor_interface.acc[1] / sensor_interface.acc[2] ), sensor_interface.gyro[0],
+ &if_angle, &if_angle_vel );
+ if_angle -= C_offset_angle * MPU9250G_DEG2RAD;
+ if_angle /= MPU9250G_DEG2RAD;
+ if_angle_vel /= MPU9250G_DEG2RAD;
+ #endif
- /*
- 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
- );
+ #ifdef USE_FIRST_IMU
+ /*
+ usb_serial.printf( "IMU 1:\t %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( "IMU1 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", posture.angle, posture.angle_vel );
+ #endif
+ #ifdef USE_SECOND_IMU
+ /*
+ usb_serial.printf( "IMU 2:%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( "IMU2 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", if_angle,if_angle_vel );
+ #endif
#endif
// control motor
@@ -261,11 +336,39 @@
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 );
- }
+ #ifdef USE_STEER_CONTROL
+ float pad_x = 0.0f;
+ pad_x = convPadJoystick( if_angle, C_dead_angle, C_max_angle );
+ comR += pad_x * control.K_rot_vel;
+ comL -= pad_x * control.K_rot_vel;
+ #endif
+
+ #ifdef USE_MOTOR_CONTROL
+ if( posture.angle < control.C_max_angle*MPU9250G_DEG2RAD && posture.angle > -control.C_max_angle*MPU9250G_DEG2RAD ){
+ controlMotor( comL, comR );
+ }else{
+ controlMotor( 0, 0 );
+ }
+ #endif
return;
+}
+
+float convPadJoystick( float input, const float val_deadzone, const float val_max ){
+ float ret = 0.0f;
+ float k = 1.0f;
+ if( val_deadzone != val_max ){
+ k = 1.0f / ( val_max - val_deadzone );
+ if( input < val_deadzone && input >-val_deadzone ){
+ ret = 0.0f;
+ }else if( input > val_max ){
+ ret = 1.0f;
+ }else if( input < -val_max ){
+ ret = -1.0f;
+ }else{
+ ret = (input > 0.0f) ? (input - val_deadzone) : (input + val_deadzone);
+ ret *= k;
+ }
+ }
+ return ret;
}
\ No newline at end of file