Diff: main.cpp

Revision:

2:9c83de2d33ca

Parent:

1:a6cb5f642e69

Child:

4:72c26c07c251

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