   {
         pCmd  = RovSerial.Get_CmdPtr(); 
 
-    /*  
-    Serial.print("Cmd : ");
-    Serial.print( pCmd->Cmd, HEX);
-    Serial.print(" Size : ");
-    Serial.print( pCmd->Length, DEC);
-    Serial.print(" ");
-    for( i=0; i<pCmd->Length; i++ )
-    {
-      Serial.print(pCmd->Data[i]);
-      Serial.print(" ");
-    }
-    Serial.println(" ");
-    */
-
-    switch( pCmd->Cmd )
+      switch( pCmd->Cmd )
     {
       case 0x01:
         IsConnected = true;

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

//----------------------------------------------------------------------------
#include <Servo.h>
#include "RSP.h"
#include "Define.h"
#include "IMU.h"
#include "LED.h"

RSP     RovSerial;
Servo   RovServo[3]; 
cIMU    IMU;
cLED    LED;

bool       IsConnected;
uint8_t    err_code;

void rc_usb_test();
void Motor_init();
void process_recv_cmd( void );

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

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

  RovSerial.begin(115200); 

  Motor_init();

  LED.Led_init();

  err_code = IMU.begin();

  IsConnected = false;
}
