 }
 
 
+void Motor_init()
+{
+  RovServo[RC_MOTOR_L].attach(RC_MOTOR_PIN_L, 1000, 2000, 0, 255 );
+  RovServo[RC_MOTOR_C].attach(RC_MOTOR_PIN_C, 1000, 2000, 0, 255 );
+  RovServo[RC_MOTOR_R].attach(RC_MOTOR_PIN_R, 1000, 2000, 0, 255 );
+
+  RovServo[RC_MOTOR_L].writeMicroseconds(1500);
+  RovServo[RC_MOTOR_C].writeMicroseconds(1500);
+  RovServo[RC_MOTOR_R].writeMicroseconds(0);
+}
+
+void sout_IMU_info()
+{
+    Serial.print(err_code);
+    Serial.print(" ");
+    Serial.print(IMU.angle[0]/10);
+    Serial.print(" ");
+    Serial.print(IMU.angle[1]/10);
+    Serial.print(" ");
+    Serial.println(IMU.angle[2]);
+}
+
+void cal_IMU_acc()
+{
+ if( Serial.available() )
+  {
+    char Ch = Serial.read();
+
+    if( Ch == '1' )
+    {
+      Serial.println("ACC Cali Start");
+
+      IMU.SEN.acc_cali_start();
+      while( IMU.SEN.acc_cali_get_done() == false )
+      {
+        IMU.update();
+        //Serial.println( IMU.SEN.calibratingA );
+      }
+
+      Serial.print("ACC Cali End ");
+    }
+  }
+}
+
+
 //---------RCtest(control Motor & LED by using USB serial)---------
 
 void rc_usb_test()

//----------------------------------------------------------------------------
#include <Servo.h>
#include "RSP.h"

RSP     RovSerial;

#define USB_TEST_AVAILABLE 1  // control Motor & LED by using USB serial

#define RC_MOTOR_L    0
#define RC_MOTOR_C    1
#define RC_MOTOR_R    2

#define RC_MOTOR_PIN_L  2
#define RC_MOTOR_PIN_C  3
#define RC_MOTOR_PIN_R  5

#define LED_PIN_L  8 
#define LED_PIN_R  9

#define MOTOR_NEUTRAL 1500

bool IsConnected;

void rc_usb_test();
void led_setup();
void process_recv_cmd( void );

void send_cmd_info( void );
void recv_cmd_control( RSP_CMD_OBJ *pCmd );

Servo   RovServo[3];  

void setup() 
{
  Serial.begin(115200);

  RovSerial.begin(115200); 
  RovServo[RC_MOTOR_L].attach(RC_MOTOR_PIN_L, 1000, 2000, 0, 255 );
  RovServo[RC_MOTOR_C].attach(RC_MOTOR_PIN_C, 1000, 2000, 0, 255 );
  RovServo[RC_MOTOR_R].attach(RC_MOTOR_PIN_R, 1000, 2000, 0, 255 );

  RovServo[RC_MOTOR_L].writeMicroseconds(1500);
  RovServo[RC_MOTOR_C].writeMicroseconds(1500);
  RovServo[RC_MOTOR_R].writeMicroseconds(0);

  pinMode(LED_PIN_L, PWM);
  pinMode(LED_PIN_R, PWM);

  analogWrite(LED_PIN_L, 0);
  analogWrite(LED_PIN_R, 0);

  IsConnected = false;
}

  //Led_Left
  if(pCmd->Data[0] == 1)
  {
    Led[0] = (LED_OFF - (pCmd->Data[2]<<8) | (pCmd->Data[1]));   
    Serial.print("Led_L : ");
    Serial.println(Led[0]);
    analogWrite(LED_PIN_L, Led[0]);
