Diff: main.cpp

Revision:

0:c86a79c8b787

Child:

1:a6cb5f642e69

--- /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;
+}