Diff: main.cpp

Revision:

4:72c26c07c251

Parent:

2:9c83de2d33ca

Child:

5:f3cbcb294ba9

--- a/main.cpp	Sat Sep 16 17:18:49 2017 +0000
+++ b/main.cpp	Mon Nov 20 00:43:51 2017 +0000
@@ -17,14 +17,14 @@
 Ticker          tic_sen_ctrl;
 CAN             can_driver( CAN_RX, CAN_TX );
 MPU9250         imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK );
-AsyncSerial     bt( BLUETOOTH_TX, BLUETOOTH_RX );
+//AsyncSerial     bt( BLUETOOTH_TX, BLUETOOTH_RX );
 //DFPlayerMini    player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
-LocalFileSystem file_local( FILESYSTEM_PATH );    
+LocalFileSystem file_local( FILESYSTEM_PATH );
 
 void beginMotor();
-//void controlMotor( int16_t left, int16_t right );
+void controlMotor( int16_t left, int16_t right );
 void sensing_and_control();
-void receive_command();
+//void receive_command();
 
 uint8_t cnt = 0;
 
@@ -45,11 +45,14 @@
     fscanf( fp, "%f\r\n", &control.K_wheel     );
     fscanf( fp, "%f\r\n", &control.K_wheel_vel );
     fscanf( fp, "%f\r\n", &control.K_rot_vel   );
-    fscanf( fp, "%f\r\n", &control.K_trans_vel );    
+    fscanf( fp, "%f\r\n", &control.K_trans_vel );
+    fscanf( fp, "%f\r\n", &control.C_max_angle );
     fclose( fp );
     
+    // coefficient
     control.K_angle     /= MPU9250G_DEG2RAD;
     control.K_angle_vel /= MPU9250G_DEG2RAD;
+    control.C_max_angle /= MPU9250G_DEG2RAD;
     
     // initialize CAN
     can_driver.frequency( 500000 );
@@ -59,8 +62,11 @@
     imu.setAccelRange( BITS_FS_16G );
     imu.setGyroRange(  BITS_FS_2000DPS );
     
+    // initialize UART for wireless
+    /*
     bt.baud( 115200 );
     bt.format( 8, RawSerial::None, 1 );
+    */
     
     // initialize player
     /*
@@ -95,8 +101,6 @@
     msg.data[1] =   0x06;
     can_driver.write( msg );
     
-    wait( 0.005f );
-    
     msg.format  =   CANStandard;
     msg.type    =   CANData;    
     msg.id      =   0x410;
@@ -126,8 +130,6 @@
     msg.data[7] =   0x00;
     can_driver.write( msg );
     
-    wait( 0.005f );
-    
     right = right;
     msg.format  =   CANStandard;
     msg.type    =   CANData;    
@@ -150,7 +152,7 @@
     //LPF variables
     int16_t com  = 0;
     int16_t comL = 0;
-    int16_t comR = 0;    
+    int16_t comR = 0;
     static float   angle_int     =   0.0f;
     static float   angle_adj     =   0.0f;
     static float   angle_adj_t   =   0.0f;
@@ -176,7 +178,7 @@
     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_vel    =   sensor.gyro[1];
@@ -211,7 +213,7 @@
       comR  -= control.K_rot_vel   * pad.x;
     }
     
-    if( posture.angle < MAX_ANGLE && posture.angle > -MAX_ANGLE ){
+    if( posture.angle < control.C_max_angle && posture.angle > -control.C_max_angle ){
         controlMotor( comL, comR );
     }else{
         controlMotor( 0, 0 );        
@@ -222,6 +224,8 @@
     return;
 }
 
+// receive command function
+/* 
 void receive_command(){
     uint8_t c = 0;
     if( bt.readable() ){
@@ -253,3 +257,4 @@
     }
     return;    
 }
+*/
\ No newline at end of file