2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

Files at this revision

API Documentation at this revision

Comitter:
bluefish
Date:
Fri Sep 15 14:54:59 2017 +0000
Child:
1:a6cb5f642e69
Commit message:
???????

Changed in this revision

AsyncSerial.lib Show annotated file Show diff for this revision Revisions of this file
Lib_DFPlayerMini.lib Show annotated file Show diff for this revision Revisions of this file
Lib_MPU9250_SPI.lib Show annotated file Show diff for this revision Revisions of this file
defines.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /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