Revision 2:9c83de2d33ca, committed 2017-09-16

Comitter:

bluefish

Date:

Sat Sep 16 17:15:30 2017 +0000

Parent:

1:a6cb5f642e69

Child:

3:7bcf3407e102

Commit message:

?????????????????????????

Changed in this revision

--- a/defines.h	Sat Sep 16 15:09:53 2017 +0000
+++ b/defines.h	Sat Sep 16 17:15:30 2017 +0000
@@ -4,7 +4,7 @@
 #include "mbed.h"
 #include "AsyncSerial.hpp"
 
-#include "Lib_DFPlayerMini.h"
+//#include "Lib_DFPlayerMini.h"
 #include "Lib_MPU9250_SPI.h"
 
 //#define USE_SERIAL_DEBUG
@@ -19,6 +19,8 @@
     DigitalOut myled[4] =   { LED1, LED2, LED3, LED4 };
 #endif
 
+#define MAX_ANGLE               ((20.0f) / MPU9250G_DEG2RAD)
+
 #define PROCESS_INTERVAL_SEC    (0.01f)
 
 #define CAN_RX                  (p30)
@@ -34,6 +36,9 @@
 #define DFPLAYER_TX             p28
 #define DFPLAYER_BUSY           p18
 
+#define BLUETOOTH_RX            p27
+#define BLUETOOTH_TX            p28
+
 #define NEOPIXEL_DOUT           (p21)
 
 #define DEVID_LEFT              (0x01)
@@ -47,7 +52,7 @@
 
 typedef struct _STRUCTPOSTURE{
     float angle;
-    float angle_vec;
+    float angle_vel;
     float wheel[2];
     float wheel_vec[2];
 } STRUCTPOSTURE;
@@ -61,9 +66,16 @@
   float   K_trans_vel;
 } STRUCTCONRTOLPARAM;
 
+typedef struct _STRUCTPAD{
+  uint8_t enable;             // 制御許可(0=制御しない 1=制御する)  
+  float   x;                  // X軸値( -1.0～1.0 0.0で中立 )
+  float   y;                  // Y軸値( -1.0～1.0 0.0で中立 )
+} STRUCTPAD;
+
 extern STRUCTSENSOR         sensor;
 extern STRUCTPOSTURE        posture;
 extern STRUCTCONRTOLPARAM   control;
+extern STRUCTPAD            pad;
 
 //extern Ticker           tic_sen_ctrl;
 //extern CAN              can_driver;

--- a/main.cpp	Sat Sep 16 15:09:53 2017 +0000
+++ b/main.cpp	Sat Sep 16 17:15:30 2017 +0000
@@ -10,17 +10,23 @@
 STRUCTSENSOR         sensor;
 STRUCTPOSTURE        posture;
 STRUCTCONRTOLPARAM   control;
+STRUCTPAD            pad;
+
 
 // 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 );
+AsyncSerial     bt( BLUETOOTH_TX, BLUETOOTH_RX );
+//DFPlayerMini    player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
 LocalFileSystem file_local( FILESYSTEM_PATH );    
 
 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;
@@ -53,6 +59,9 @@
     imu.setAccelRange( BITS_FS_16G );
     imu.setGyroRange(  BITS_FS_2000DPS );
     
+    bt.baud( 115200 );
+    bt.format( 8, RawSerial::None, 1 );
+    
     // initialize player
     /*
     player.begin();
@@ -139,7 +148,9 @@
 
 void sensing_and_control(){
     //LPF variables
-    int16_t com = 0;
+    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;
@@ -168,20 +179,77 @@
     
     // update state
     posture.angle        =   angle_int + angle_adj;
-    posture.angle_vec    =   sensor.gyro[1];
+    posture.angle_vel    =   sensor.gyro[1];
+    
+    if( cnt > 0 ){ cnt--; }
     
     #ifdef USE_SERIAL_DEBUG
         usb_serial.printf( "%f\t %f\n", 
             posture.angle / MPU9250G_DEG2RAD, 
             posture.angle_vec / MPU9250G_DEG2RAD        
         );
-    #endif    
+    #endif
     
     // control motor
-    com = (int16_t)( 
-          posture.angle      * control.K_angle 
-        + posture.angle_vec  * control.K_angle_vel );
-    controlMotor( com, com );
+    com  =  0;
+    com +=  control.K_angle      * posture.angle;
+    com +=  control.K_angle_vel  * posture.angle_vel;
+
+    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 < MAX_ANGLE && posture.angle > -MAX_ANGLE ){
+        controlMotor( comL, comR );
+    }else{
+        controlMotor( 0, 0 );        
+    }
+    
+    receive_command();
     
     return;
 }
+
+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;    
+}