2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Revision 0:c86a79c8b787, committed 2017-09-15
- Comitter:
- bluefish
- Date:
- Fri Sep 15 14:54:59 2017 +0000
- Child:
- 1:a6cb5f642e69
- Commit message:
- ???????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AsyncSerial.lib Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/babylonica/code/AsyncSerial/#54cbbb00bd19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Lib_DFPlayerMini.lib Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,1 @@ +Lib_DFPlayerMini#6e015ec7e3a7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Lib_MPU9250_SPI.lib Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,1 @@ +Lib_MPU9250_SPI#551dbbd41a10
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/defines.h Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,73 @@ +#ifndef __DEFINES_H__ +#define __DEFINES_H__ + +#include "mbed.h" +#include "AsyncSerial.hpp" + +#include "Lib_DFPlayerMini.h" +#include "Lib_MPU9250_SPI.h" + +//#define USE_SERIAL_DEBUG +//#define USE_LED_DEBUG + +#ifdef USE_SERIAL_DEBUG + #define DEBUG_BAUDRATE 115200 + AsyncSerial usb_serial( USBTX, USBRX ); +#endif + +#ifdef USE_LED_DEBUG + DigitalOut myled[4] = { LED1, LED2, LED3, LED4 }; +#endif + +#define PROCESS_INTERVAL_SEC (0.01f) + +#define CAN_RX (p30) +#define CAN_TX (p29) + +#define MPU9250_CS p7 +#define MPU9250_MOSI p11 +#define MPU9250_MISO p12 +#define MPU9250_SCK p13 +#define MPU9250_INT p8 + +#define DFPLAYER_RX p27 +#define DFPLAYER_TX p28 +#define DFPLAYER_BUSY p18 + +#define NEOPIXEL_DOUT (p21) + +#define DEVID_LEFT (0x01) +#define DEVID_RIGHT (0x04) + +typedef struct _STRUCTSENSOR{ + float acc[3]; + float gyro[3]; + float mag[3]; +} STRUCTSENSOR; + +typedef struct _STRUCTPOSTURE{ + float angle; + float angle_vec; + float wheel[2]; + float wheel_vec[2]; +} STRUCTPOSTURE; + +typedef struct _STRUCTCONRTOLPARAM{ + float K_angle; + float K_angle_vel; + float K_wheel; + float K_wheel_vel; + float K_rot_vel; + float K_trans_vel; +} STRUCTCONRTOLPARAM; + +extern STRUCTSENSOR sensor; +extern STRUCTPOSTURE posture; +extern STRUCTCONRTOLPARAM control; + +//extern Ticker tic_sen_ctrl; +//extern CAN can_driver; +//extern MPU9250 imu; +//extern DFPlayerMini player; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,175 @@ +#include "mbed.h" +#include "defines.h" + +#define K_ANGLE (1000.0f) +#define K_ANGLE_VEL (5.0f) + +STRUCTSENSOR sensor; +STRUCTPOSTURE posture; +STRUCTCONRTOLPARAM control; + + + +// class instance +Ticker tic_sen_ctrl; +CAN can_driver( CAN_RX, CAN_TX ); +MPU9250 imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK ); +DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX ); + +void beginMotor(); +//void controlMotor( int16_t left, int16_t right ); +void sensing_and_control(); + +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 + control.K_angle = K_ANGLE / MPU9250G_DEG2RAD; + control.K_angle_vel = 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 ); + + // 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; + 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_vec = sensor.gyro[1]; + + #ifdef USE_SERIAL_DEBUG + usb_serial.printf( "%f\t %f\n", + posture.angle / MPU9250G_DEG2RAD, + posture.angle_vec / MPU9250G_DEG2RAD + ); + #endif + + // control motor + com = (int16_t)( + posture.angle * control.K_angle + + posture.angle_vec * control.K_angle_vel ); + controlMotor( com, com ); + + return; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/e2bfab296f20 \ No newline at end of file