diff --git a/tfrog-motordriver/communication.c b/tfrog-motordriver/communication.c
index b7a75a4..15f315d 100644
--- a/tfrog-motordriver/communication.c
+++ b/tfrog-motordriver/communication.c
@@ -51,6 +51,7 @@ extern Tfrog_EEPROM_data saved_param;
 extern volatile char rs485_timeout;
 extern volatile short tic;
 extern volatile unsigned char usb_read_pause;
+volatile int usb_timeout_cnt = 0;
 
 unsigned short crc16(unsigned char* buf, int len) RAMFUNC;
 unsigned char crc8(unsigned char* buf, int len) RAMFUNC;
@@ -216,15 +217,14 @@ int nsend(char* buf, int len)
 
 void flush(void)
 {
-  int len;
+  const int len = send_buf_pos;
   char ret;
-  unsigned short timeout;
 
-  len = send_buf_pos;
-  send_buf[len] = 0;
   if (len == 0)
     return;
-  for (timeout = 1024; timeout; timeout--)
+
+  const unsigned short tic_start = tic;
+  while ((unsigned short)(tic - tic_start) < 46)
   {
     ret = CDCDSerialDriver_Write(send_buf, len, 0, 0);
     if (ret == USBD_STATUS_LOCKED)
@@ -233,29 +233,26 @@ void flush(void)
     }
     else if (ret != USBD_STATUS_SUCCESS)
     {
-      TRACE_ERROR("Send failed\n\r  buf: %s\n\r", send_buf);
+      printf("USB:w err (%d)\n\r", ret);
       send_buf_pos = 0;
       return;
     }
     else
     {
       send_buf_pos -= len;
-      break;
+      return;
     }
   }
-  if (!timeout)
-  {
-    TRACE_ERROR("USB send timeout\n\r");
-    send_buf_pos = 0;
-  }
+  usb_timeout_cnt++;
+  send_buf_pos = 0;
 }
 void flush485(void)
 {
   if (send_buf_pos485 == 0)
     return;
 
-  tic = 0;
-  while (tic < 10)
+  const unsigned short tic_start = tic;
+  while ((unsigned short)(tic - tic_start) < 10)
   {
     if (USART_WriteBuffer(AT91C_BASE_US0, send_buf485, send_buf_pos485))
     {
@@ -269,7 +266,7 @@ void flush485(void)
       return;
     }
   }
-  printf("RS485 send timeout\n\r");
+  printf("485:w timeout\n\r");
   send_buf_pos485 = 0;
   rs485_timeout = 0;
 }
@@ -361,10 +358,10 @@ int decord(unsigned char* src, int len, unsigned char* dst, int buf_max)
 
 short rs485_timeout_wait(unsigned char t, unsigned short timeout)
 {
-  tic = 0;
+  const unsigned short tic_start = tic;
   while (rs485_timeout < t)
   {
-    if (tic > timeout)
+    if ((unsigned short)(tic - tic_start) > timeout)
       return 0;
   }
   return 1;
@@ -430,7 +427,7 @@ int data_send485(short* cnt, short* pwm, char* en, short* analog, unsigned short
   {
     send_buf_pos485 = 0;
     rs485_timeout = 0;
-    printf("rs485 skipped\n\r");
+    printf("485:skip\n\r");
     return -1;
   }
 }
@@ -511,7 +508,7 @@ int int_send485(const char param, const char id, const int value)
   {
     send_buf_pos485 = 0;
     rs485_timeout = 0;
-    printf("rs485 skipped\n\r");
+    printf("485:skip\n\r");
     return -1;
   }
 }
@@ -644,19 +641,9 @@ static inline int data_analyze_(
     line[len] = *data;
     len++;
 
-    char clear_buffer = 0;
     if (len > 63)
     {
-      clear_buffer = 1;
-      printf("recv buf overflow\n\r");
-    }
-    if (!fromto && usb_read_pause == 1 && len > COMMAND_LEN * 2)
-    {
-      clear_buffer = 1;
-      printf("clearing recv buf (USB buffer overflow (%d))\n\r", len);
-    }
-    if (clear_buffer)
-    {
+      printf("COM:rbuf ovf %d\n\r", len);
       len = 0;
       receive_period = 1;
       state = STATE_IDLE;
@@ -756,8 +743,7 @@ static inline int data_analyze_(
         {
           state = STATE_IDLE;
           receive_period = 1;
-          line[len - 3] = 0;
-          printf("CRC16 X\"%s\"\n\r", (char*)line);
+          printf("COM:CRC err (%d)\n\r", len);
         }
         break;
     }
@@ -771,8 +757,7 @@ static inline int data_analyze_(
         data_len = decord(line, len - 1, rawdata, 16);
         if (data_len < 6)
         {
-          line[len - 1] = 0;
-          printf("Decode failed: \"%s\" (%d)\n\r", (char*)line, data_len);
+          printf("COM:decode err (%d)\n\r", data_len);
         }
         else
         {
@@ -821,14 +806,12 @@ static inline int data_analyze_(
               {
                 send_buf_pos485 = 0;
                 rs485_timeout = 0;
-                printf("rs485 skipped\n\r");
+                printf("485:skip\n\r");
               }
-              //printf("proxy sent\n\r");
             }
             else
             {
               send_buf_pos485--;
-              //printf("proxy\n\r");
             }
           }
         }
@@ -889,9 +872,8 @@ static inline int data_analyze_(
     {
       send_buf_pos485 = 0;
       rs485_timeout = 0;
-      printf("rs485 skipped\n\r");
+      printf("485:skip\n\r");
     }
-    //printf("proxy sent\n\r");
   }
   return 0;
 }
@@ -1477,9 +1459,16 @@ int extended_command_analyze(char* data)
     }
     else
     {
-      printf("unknown command \"%s\"\n\r", data);
       send(data);
       send("\n0Ee\n\n");
+
+      char* d;
+      for (d = data; *d != 0; ++d)
+      {
+        if (*d < 0x20 || 0x7e < *d)
+          *d = '?';
+      }
+      printf("unknown command \"%s\"\n\r", data);
     }
   }
   flush();
diff --git a/tfrog-motordriver/communication.h b/tfrog-motordriver/communication.h
index b96d4b3..d0b611b 100644
--- a/tfrog-motordriver/communication.h
+++ b/tfrog-motordriver/communication.h
@@ -165,6 +165,8 @@ void sendclear(void);
 void flush(void);
 void flush485(void);
 
+extern volatile int usb_timeout_cnt;
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/tfrog-motordriver/controlPWM.c b/tfrog-motordriver/controlPWM.c
index d108732..f297196 100644
--- a/tfrog-motordriver/controlPWM.c
+++ b/tfrog-motordriver/controlPWM.c
@@ -35,12 +35,14 @@
 #include "controlVelocity.h"
 #include "power.h"
 #include "eeprom.h"
+#include "utils.h"
 
 static const Pin pinPWMCycle2 = PIN_PWM_CYCLE2;
 
 // / PWM Enable pin instance.
 static const Pin pinPWMEnable = PIN_PWM_ENABLE;
 
+static char init = 1;
 short SinTB[1024];
 int PWM_abs_max = 0;
 int PWM_abs_min = 0;
@@ -67,34 +69,6 @@ inline short sin_(int x)
 extern Tfrog_EEPROM_data saved_param;
 extern volatile char rs485_timeout;
 
-inline void normalize(int* val, int min, int max, int resolution)
-{
-  if (resolution <= 0)
-    return;
-  while (*val < min)
-    *val += resolution;
-  while (*val >= max)
-    *val -= resolution;
-}
-
-inline void normalize_mod(int* val, int min, int max, int resolution)
-{
-  if (resolution <= 0)
-    return;
-  *val = (*val) % resolution;
-  if (*val < min)
-    *val += resolution;
-  else if (*val >= max)
-    *val -= resolution;
-}
-
-inline int _abs(int x)
-{
-  if (x < 0)
-    return -x;
-  return x;
-}
-
 void controlPWM_config(int i)
 {
   switch (motor_param[i].motor_type)
@@ -119,7 +93,7 @@ void controlPWM_config(int i)
 
   motor[i].ref.rate = 0;
 
-  motor_param[i].enc_rev = (int)motor_param[i].enc_rev_raw / (int)motor_param[i].enc_denominator;
+  motor_param[i].enc_rev = motor_param[i].enc_rev_raw / motor_param[i].enc_denominator;
 
   motor_param[i].enc_drev[0] = motor_param[i].enc_rev * 1 / 6;
   motor_param[i].enc_drev[1] = motor_param[i].enc_rev * 2 / 6;
@@ -129,13 +103,13 @@ void controlPWM_config(int i)
   motor_param[i].enc_drev[5] = motor_param[i].enc_rev * 6 / 6;
 
   motor_param[i].enc_10hz = motor_param[i].enc_rev * 10 * 16 / 1000;
-  motor_param[i].enc_rev_1p = motor_param[i].enc_rev / 100;
+  motor_param[i].enc_rev_1p = motor_param[i].enc_rev / 300;
   if (motor_param[i].enc_rev_1p == 0)
     motor_param[i].enc_rev_1p = 1;
 
   motor_param[i].enc_mul =
-      (unsigned int)((uint64_t)SinTB_2PI * 0x40000 * motor_param[i].enc_denominator /
-                     motor_param[i].enc_rev_raw);
+      (int)((int64_t)SinTB_2PI * 0x40000 * motor_param[i].enc_denominator /
+            motor_param[i].enc_rev_raw);
   motor_param[i].enc_rev_h = motor_param[i].enc_rev / 2;
 
   // normalize phase offset
@@ -184,6 +158,7 @@ void controlPWM_config(int i)
     }
   }
   motor_param[i].enc0tran = motor_param[i].enc0;
+  init = 1;
 }
 
 // ------------------------------------------------------------------------------
@@ -192,14 +167,13 @@ void controlPWM_config(int i)
 void FIQ_PWMPeriod()
 {
   int i;
-  unsigned short enc[2];
-  unsigned short hall[2];
-  static unsigned short _enc[2];
-  static unsigned short _hall[2];
-  static int init = 0;
-  static int cnt = 0;
-  cnt++;
+  static unsigned short enc[2];
+  static unsigned short hall[2];
+  static unsigned int cnt = 0;
+  unsigned short _enc[2];
+  unsigned short _hall[2];
 
+  cnt++;
   {
     // PWM周波数が高い場合は処理を間引く
     // PWM_resolution 2000以下で一回間引き
@@ -211,6 +185,11 @@ void FIQ_PWMPeriod()
     thin = 0;
   }
 
+  _enc[0] = enc[0];
+  _enc[1] = enc[1];
+  _hall[0] = hall[0];
+  _hall[1] = hall[1];
+
   for (i = 0; i < 2; i++)
   {
     hall[i] = *(unsigned short*)&THEVA.MOTOR[i].ROT_DETECTER;
@@ -246,32 +225,32 @@ void FIQ_PWMPeriod()
       THEVA.MOTOR[i % 2].PWM[i / 2].H = PWM_resolution;
       THEVA.MOTOR[i % 2].PWM[i / 2].L = PWM_resolution;
     }
-    init = 0;
+    init = 1;
     disabled = 1;
   }
-  else if (!init)
+  else if (init)
   {
-    init = 1;
-    _hall[0] = hall[0];
-    _hall[1] = hall[1];
-    _enc[0] = enc[0];
-    _enc[1] = enc[1];
-
+    init = 0;
     return;
   }
 
   for (i = 0; i < 2; i++)
   {
+    if (motor[i].servo_level == SERVO_LEVEL_STOP)
+      continue;
+
     if (motor_param[i].enc_type == 2 ||
         motor_param[i].enc_type == 0)
     {
-      motor[i].pos += (short)(enc[i] - _enc[i]);
+      const short diff = (short)(enc[i] - _enc[i]);
+      motor[i].pos += diff;
+      normalize(&motor[i].pos, 0, motor_param[i].enc_rev_raw, motor_param[i].enc_rev_raw);
+
       motor[i].posc = (motor[i].posc & 0xFFFF0000) | enc[i];
       if (_enc[i] < 0x4000 && enc[i] > 0xC000)
         motor[i].posc -= 0x10000;
       else if (_enc[i] > 0xC000 && enc[i] < 0x4000)
         motor[i].posc += 0x10000;
-      normalize(&motor[i].pos, 0, motor_param[i].enc_rev_raw, motor_param[i].enc_rev_raw);
     }
   }
 
@@ -303,30 +282,6 @@ void FIQ_PWMPeriod()
         motor[j].ref.rate2 = rate;
       }
 
-      if (cnt % 64 == 2 + j)
-      {
-        int diff;
-        diff = motor_param[j].enc0tran - motor_param[j].enc0;
-        normalize(&diff, -motor_param[j].enc_rev_h, motor_param[j].enc_rev_h, motor_param[j].enc_rev);
-
-        if (_abs(diff) <= motor_param[j].enc_rev_1p)
-        {
-          motor_param[j].enc0tran = motor_param[j].enc0;
-        }
-        else if (diff > 0)
-        {
-          motor_param[j].enc0tran -= motor_param[j].enc_rev_1p;
-          if (motor_param[j].enc0tran <= -motor_param[j].enc_rev)
-            motor_param[j].enc0tran += motor_param[j].enc_rev;
-        }
-        else
-        {
-          motor_param[j].enc0tran += motor_param[j].enc_rev_1p;
-          if (motor_param[j].enc0tran >= motor_param[j].enc_rev)
-            motor_param[j].enc0tran -= motor_param[j].enc_rev;
-        }
-      }
-
       switch (motor_param[j].motor_type)
       {
         case MOTOR_TYPE_DC:
@@ -413,11 +368,13 @@ void FIQ_PWMPeriod()
   // ゼロ点計算
   for (i = 0; i < 2; i++)
   {
-    int u, v, w;
+    if (motor[i].servo_level == SERVO_LEVEL_STOP)
+      continue;
 
     if (motor_param[i].motor_type != MOTOR_TYPE_DC &&
         motor_param[i].enc_type != 0)
     {
+      int u, v, w;
       char dir;
       unsigned short halldiff;
 
@@ -550,9 +507,9 @@ void FIQ_PWMPeriod()
         motor[i].enc += diff;
 
         if (diff > 0)
-          motor[i].spd = PWM_cpms / (cnt - motor[i].spd_cnt);
+          motor[i].spd = PWM_cpms / (int)(cnt - motor[i].spd_cnt);
         else
-          motor[i].spd = -PWM_cpms / (cnt - motor[i].spd_cnt);
+          motor[i].spd = -PWM_cpms / (int)(cnt - motor[i].spd_cnt);
         motor[i].spd_cnt = cnt;
         continue;
       }
@@ -574,10 +531,10 @@ void FIQ_PWMPeriod()
         enc0 = motor[i].pos - motor_param[i].enc_drev[5] + dir - 1;
 
       // Check hall signal consistency
-      if (motor_param[i].enc_type == 2 && motor[i].servo_level > SERVO_LEVEL_STOP)
+      if (motor_param[i].enc_type == 2)
       {
         int err = motor_param[i].enc0 - enc0;
-        normalize_mod(&err, -motor_param[i].enc_rev_h, motor_param[i].enc_rev_h, motor_param[i].enc_rev);
+        normalize(&err, -motor_param[i].enc_rev_h, motor_param[i].enc_rev_h, motor_param[i].enc_rev);
         // In worst case, initial encoder origin can have offset of motor_param[i].enc_rev/12.
         if (_abs(err) > motor_param[i].enc_rev / 6)
         {
@@ -595,12 +552,6 @@ void FIQ_PWMPeriod()
     }
   }
 
-  _hall[0] = hall[0];
-  _hall[1] = hall[1];
-
-  _enc[0] = enc[0];
-  _enc[1] = enc[1];
-
   return;
 }
 
@@ -630,7 +581,7 @@ void controlPWM_init()
   }
 
   PIO_Configure(&pinPWMCycle2, 1);
-  AIC_ConfigureIT(AT91C_ID_IRQ0, 5 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))FIQ_PWMPeriod);
+  AIC_ConfigureIT(AT91C_ID_IRQ0, 6 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))FIQ_PWMPeriod);
   AIC_EnableIT(AT91C_ID_IRQ0);
 
   // PWM Generator init
diff --git a/tfrog-motordriver/controlPWM.h b/tfrog-motordriver/controlPWM.h
index 76f88d8..657254c 100644
--- a/tfrog-motordriver/controlPWM.h
+++ b/tfrog-motordriver/controlPWM.h
@@ -19,9 +19,7 @@
 #define __CONTROL_PWM_H__
 
 void controlPWM_init();
-void controlPWM_config();
+void controlPWM_config(int i);
 void FIQ_PWMPeriod();
-int _abs(int x);
-void normalize(int* val, int min, int max, int resolution);
 
 #endif
diff --git a/tfrog-motordriver/controlVelocity.c b/tfrog-motordriver/controlVelocity.c
index 12ae8df..d19c8ed 100644
--- a/tfrog-motordriver/controlVelocity.c
+++ b/tfrog-motordriver/controlVelocity.c
@@ -34,6 +34,7 @@
 #include "controlVelocity.h"
 #include "registerFPGA.h"
 #include "filter.h"
+#include "utils.h"
 
 MotorState motor[2];
 MotorParam motor_param[2];
@@ -60,7 +61,6 @@ void timer0_vel_calc() RAMFUNC;
 // ------------------------------------------------------------------------------
 void ISR_VelocityControl()
 {
-  // volatile unsigned int status;
   int i;
   int64_t toq[2];
   int64_t out_pwm[2];
@@ -246,24 +246,27 @@ void ISR_VelocityControl()
 
 void timer0_vel_calc()
 {
-  static unsigned int __enc[2];
-  unsigned int enc[2];
+  static unsigned int enc[2];
   static char _spd_cnt[2];
+  unsigned int __enc[2];
   int spd[2];
   int i;
   volatile unsigned int dummy;
 
+  dummy = AT91C_BASE_TC0->TC_SR;
+  dummy = dummy;
+
   if (motor[0].servo_level > SERVO_LEVEL_STOP ||
       motor[1].servo_level > SERVO_LEVEL_STOP)
   {
     driver_state.watchdog++;
-    driver_state.cnt_updated++;
+    if (driver_state.cnt_updated < 9)
+      driver_state.cnt_updated++;
   }
 
-  dummy = AT91C_BASE_TC0->TC_SR;
-  dummy = dummy;
-
   LED_on(1);
+  __enc[0] = enc[0];
+  __enc[1] = enc[1];
   for (i = 0; i < 2; i++)
   {
     enc[i] = motor[i].posc;
@@ -271,6 +274,7 @@ void timer0_vel_calc()
     motor[i].spd = 0;
   }
 
+  // calculate motor velocities
   for (i = 0; i < 2; i++)
   {
     int __vel;
@@ -310,6 +314,12 @@ void timer0_vel_calc()
       motor[i].dir = 0;
     }
 
+    if (motor[i].control_init)
+    {
+      vel = 0;
+      motor[i].dir = 0;
+    }
+
     if (motor_param[i].enc_type == 0)
     {
       motor[i].vel = motor[i].ref.vel;
@@ -328,26 +338,46 @@ void timer0_vel_calc()
     {
       motor[i].vel = vel;
     }
-    __enc[i] = enc[i];
     motor[i].enc_buf = enc[i] >> motor_param[i].enc_div;
   }
   for (i = 0; i < 2; i++)
   {
-    if (motor[i].servo_level >= SERVO_LEVEL_TORQUE)
+    if (motor[i].servo_level == SERVO_LEVEL_STOP)
+      continue;
+
+    if (driver_state.cnt_updated == 5)
     {
-      if (driver_state.cnt_updated == 5)
-      {
-        motor[i].ref.rate_buf = pwm_sum[i] * 5 / pwm_num[i];
-        motor[i].enc_buf2 = motor[i].enc_buf;
-        pwm_sum[i] = 0;
-        pwm_num[i] = 0;
-      }
+      motor[i].ref.rate_buf = pwm_sum[i] * 5 / pwm_num[i];
+      motor[i].enc_buf2 = motor[i].enc_buf;
+      pwm_sum[i] = 0;
+      pwm_num[i] = 0;
     }
   }
 
+  // encoder absolute angle LPF
+  for (i = 0; i < 2; i++)
+  {
+    if (motor[i].servo_level == SERVO_LEVEL_STOP)
+      continue;
+
+    int enc0 = motor_param[i].enc0;
+    int diff;
+    diff = motor_param[i].enc0tran - enc0;
+    normalize(&diff, -motor_param[i].enc_rev_h, motor_param[i].enc_rev_h, motor_param[i].enc_rev);
+
+    if (_abs(diff) <= motor_param[i].enc_rev_1p)
+      motor_param[i].enc0tran = enc0;
+    else if (diff > 0)
+      motor_param[i].enc0tran -= motor_param[i].enc_rev_1p;
+    else
+      motor_param[i].enc0tran += motor_param[i].enc_rev_1p;
+
+    normalize(&motor_param[i].enc0tran, 0, motor_param[i].enc_rev_raw, motor_param[i].enc_rev_raw);
+  }
+  ISR_VelocityControl();
   LED_off(1);
 
-  driver_state.velcontrol = 1;
+  driver_state.velcontrol++;
 }
 
 // ------------------------------------------------------------------------------
@@ -376,7 +406,7 @@ void controlVelocity_init()
     motor[i].ref.vel_buf_prev = 0;
     motor[i].ref.vel_diff = 0;
     motor[i].error_integ = 0;
-    motor[i].control_init = 0;
+    motor[i].control_init = 1;
     motor[i].servo_level = SERVO_LEVEL_STOP;
     motor_param[i].motor_type = MOTOR_TYPE_AC3;
     motor_param[i].rotation_dir = 1;
diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h
index 6313400..585a9a1 100644
--- a/tfrog-motordriver/controlVelocity.h
+++ b/tfrog-motordriver/controlVelocity.h
@@ -41,14 +41,14 @@ typedef enum _ErrorID
 
 typedef struct _MotorState
 {
-  int vel;            // count/ms
-  int vel1;           // count/ms
-  int pos;            // count
-  unsigned int posc;  // count
-  int enc_buf;        // count
-  int enc_buf2;       // count
+  int vel;                // count/ms
+  int vel1;               // count/ms
+  int pos;                // count
+  unsigned int posc;      // count
+  unsigned int enc_buf;   // count
+  unsigned int enc_buf2;  // count
   int spd;
-  int spd_cnt;
+  unsigned int spd_cnt;
   unsigned short enc;
   short dir;
 
@@ -75,14 +75,14 @@ typedef struct _MotorState
 
 typedef struct _MotorParam
 {
-  unsigned int enc_rev;  // count/rev
-  unsigned int phase_offset;
-  unsigned int enc_rev_h;  // count/rev
-  unsigned int enc_rev_1p;
+  int enc_rev;  // count/rev
+  int phase_offset;
+  int enc_rev_h;  // count/rev
+  int enc_rev_1p;
   unsigned char enc_type;
-  unsigned int enc_10hz;
-  unsigned int enc_drev[6];
-  unsigned int enc_mul;
+  int enc_10hz;
+  int enc_drev[6];
+  int enc_mul;
   int enc0;      // count
   int enc0tran;  // count
   int vel_max;   // count/ms
@@ -106,9 +106,9 @@ typedef struct _MotorParam
     MOTOR_TYPE_DC,
     MOTOR_TYPE_AC3
   } motor_type;
-  unsigned short enc_div;
-  unsigned short enc_denominator;
-  unsigned int enc_rev_raw;  // count/rev
+  short enc_div;
+  short enc_denominator;
+  int enc_rev_raw;  // count/rev
   char rotation_dir;
 } MotorParam;
 
diff --git a/tfrog-motordriver/main.c b/tfrog-motordriver/main.c
index 7c2d7e4..9a36655 100644
--- a/tfrog-motordriver/main.c
+++ b/tfrog-motordriver/main.c
@@ -81,7 +81,6 @@
 #warning "T-frog driver rev.4"
 #endif
 
-int velcontrol = 0;
 volatile unsigned char rs485_timeout = 0;
 volatile unsigned short tic = 0;
 
@@ -214,14 +213,8 @@ static void ISR_Vbus( const Pin * pPin )
 // ------------------------------------------------------------------------------
 static void VBus_Configure(void)
 {
-  //	TRACE_INFO( "VBus configuration\n\r" );
-
   // Configure PIO
   PIO_Configure(&pinVbus, 1);
-  //	PIO_ConfigureIt( &pinVbus, ISR_Vbus );
-  //	PIO_EnableIt( &pinVbus );
-
-  //	ISR_Vbus( &pinVbus );
 }
 
 volatile unsigned char usb_read_pause = 0;
@@ -238,7 +231,7 @@ static void UsbDataReceived(unsigned int unused, unsigned char status, unsigned
     // Check if bytes have been discarded
     if ((received == DATABUFFERSIZE) && (remaining > 0))
     {
-      TRACE_WARNING("UsbDataReceived: %u bytes discarded\n\r", remaining);
+      printf("USB:discard %uB\n\r", remaining);
     }
 
     LED_on(2);
@@ -246,12 +239,12 @@ static void UsbDataReceived(unsigned int unused, unsigned char status, unsigned
 
     if (remain > 0)
     {
-      printf("%d bytes discarded\n\r", remain);
+      printf("USB:remain %dB\n\r", remain);
     }
 
     if (buf_left() < COMMAND_LEN * 2)
     {
-      printf("Buffer nealy full\n\rPause USB read\n\r");
+      printf("USB:pause\n\r");
       usb_read_pause = 1;
     }
     else
@@ -261,7 +254,7 @@ static void UsbDataReceived(unsigned int unused, unsigned char status, unsigned
   }
   else
   {
-    TRACE_WARNING("UsbDataReceived: Transfer error\n\r");
+    printf("USB:transfer err\n\r");
   }
 }
 
@@ -317,7 +310,7 @@ void tic_init()
   AT91C_BASE_TC1->TC_RC = 1500 / 23;
   AT91C_BASE_TC1->TC_IER = AT91C_TC_CPCS;
 
-  AIC_ConfigureIT(AT91C_ID_TC1, 3 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))timer1_tic);
+  AIC_ConfigureIT(AT91C_ID_TC1, 5 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))timer1_tic);
   AIC_EnableIT(AT91C_ID_TC1);
 
   AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
@@ -338,7 +331,7 @@ int main()
   int vbuslv = 0;
   int vbus = 0;
   int _vbus = 0;
-  int err_chk = 0;
+  unsigned int err_chk = 0;
   short mscnt = 0;
   unsigned char errnum = 0;
   unsigned char blink = 0;
@@ -441,7 +434,7 @@ int main()
     PIO_Configure(pinsSet, PIO_LISTSIZE(pinsSet));
 #endif
 
-    TRACE_ERROR("Invalid FPGA %u !\n\r", THEVA.GENERAL.ID);
+    printf("Invalid FPGA %u !\n\r", THEVA.GENERAL.ID);
     for (i = 0; i < 30000; i++)
       ;
     err_cnt++;
@@ -533,7 +526,7 @@ int main()
       case -2:
       case -3:
         // Read Error
-        TRACE_ERROR("EEPROM Read Error!\n\r");
+        printf("EEPROM Read Error!\n\r");
         motor[0].error_state |= ERROR_EEPROM;
         motor[1].error_state |= ERROR_EEPROM;
         break;
@@ -673,7 +666,7 @@ int main()
 
   AT91C_BASE_US0->US_IDR = 0xFFFFFFFF;
   AT91C_BASE_US0->US_IER = AT91C_US_RXRDY;
-  AIC_ConfigureIT(AT91C_ID_US0, 4 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))us0_received);
+  AIC_ConfigureIT(AT91C_ID_US0, 3 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))us0_received);
   AIC_EnableIT(AT91C_ID_US0);
 
   {
@@ -702,6 +695,7 @@ int main()
   driver_state.board_version = BOARD_R4;
   printf("Board version: R4\n\r");
 #endif
+  printf("Entering main control loop\n\r------\n\r");
 
   ADC_Start();
   LED_off(0);
@@ -731,7 +725,7 @@ int main()
       }
       if (err_cnt > 3)
       {
-        TRACE_ERROR("FPGA-Value Error!\n\r");
+        printf("FPGA-Value Error!\n\r");
         msleep(50);
         AT91C_BASE_RSTC->RSTC_RCR = 0xA5000000 | AT91C_RSTC_EXTRST | AT91C_RSTC_PROCRST | AT91C_RSTC_PERRST;
         while (1)
@@ -741,6 +735,9 @@ int main()
 
     if (driver_state.watchdog >= driver_param.watchdog_limit)
     {
+      motor[0].servo_level = SERVO_LEVEL_STOP;
+      motor[1].servo_level = SERVO_LEVEL_STOP;
+
       if (saved_param.stored_data == TFROG_EEPROM_DATA_BIN_SAVING)
       {
         LED_on(0);
@@ -766,7 +763,7 @@ int main()
       driver_state.error.hall[1] = 0;
       motor[0].error_state |= ERROR_WATCHDOG;
       motor[1].error_state |= ERROR_WATCHDOG;
-      TRACE_ERROR("Watchdog - parameter init\n\r");
+      printf("Watchdog - init parameters\n\r");
       {
         int i;
         printf("Motors: ");
@@ -783,8 +780,6 @@ int main()
         printf("\n\r");
         com_en[0] = com_en[1] = 1;
       }
-      motor[0].servo_level = SERVO_LEVEL_STOP;
-      motor[1].servo_level = SERVO_LEVEL_STOP;
       driver_state.watchdog = 0;
       if (!(saved_param.stored_data == TFROG_EEPROM_DATA_BIN ||
             saved_param.stored_data == TFROG_EEPROM_DATA_BIN_LOCKED))
@@ -805,34 +800,29 @@ int main()
     // Check current level on VBus
     if (PIO_Get(&pinVbus))
     {
-      if (vbuslv < 0)
-        vbuslv = 0;
-      else if (vbuslv <= 10)
+      if (vbuslv < 10)
         vbuslv++;
+      else if (vbuslv == 10)
+        vbus = 1;
     }
     else
     {
-      if (vbuslv > 0)
-        vbuslv = 0;
-      else if (vbuslv >= -10)
+      if (vbuslv > -10)
         vbuslv--;
+      else if (vbuslv == -10)
+        vbus = 0;
     }
-    if (vbuslv > 10)
-      vbus = 1;
-    else if (vbuslv < -10)
-      vbus = 0;
-
     if (vbus != _vbus)
     {
       if (vbus == 1)
       {
-        TRACE_INFO("VBUS conn\n\r");
+        printf("USB:vbus connect\n\r");
         USBD_Connect();
         connecting = 1;
       }
       else
       {
-        TRACE_INFO("VBUS discon\n\r");
+        printf("USB:vbus disconnect\n\r");
         USBD_Disconnect();
       }
     }
@@ -848,7 +838,7 @@ int main()
       len = RS485BUF_SIZE - r_rs485buf_pos;
       if (data_fetch485(rs485buf + r_rs485buf_pos, len))
       {
-        TRACE_WARNING("RS485DataReceived: buffer overrun\n\r");
+        printf("485:buf ovf\n\r");
       }
 
       if (rs485buf == &rs485buf_[0][0])
@@ -876,7 +866,7 @@ int main()
 
         if (data_fetch485(rs485buf + r_rs485buf_pos, len))
         {
-          TRACE_WARNING("RS485DataReceived: buffer overrun\n\r");
+          printf("485:buf ovf\n\r");
         }
         r_rs485buf_pos += len;
       }
@@ -967,15 +957,15 @@ int main()
 
     if (usb_read_pause)
     {
-      printf("Flushing commands\n\r");
+      printf("USB:flush\n\r");
     }
     data_analyze();
 
     if (usb_read_pause)
     {
       usb_read_pause = 0;
+      printf("USB:resume\n\r");
       CDCDSerialDriver_Read(usbBuffer, DATABUFFERSIZE, (TransferCallback)UsbDataReceived, 0);
-      printf("Resume USB read\n\r");
     }
 
     data_analyze485();
@@ -984,7 +974,7 @@ int main()
     {
       if (USBD_GetState() >= USBD_STATE_CONFIGURED)
       {
-        printf("Start USB read\n\r");
+        printf("USB:start\n\r");
         // Start receiving data on the USB
         CDCDSerialDriver_Read(usbBuffer, DATABUFFERSIZE, (TransferCallback)UsbDataReceived, 0);
         connecting = 0;
@@ -995,7 +985,7 @@ int main()
     {
       if (USBD_GetState() < USBD_STATE_DEFAULT)
       {
-        TRACE_ERROR("USB disconnected\n\r");
+        printf("USB:disconnect\n\r");
         AT91C_BASE_RSTC->RSTC_RCR = 0xA5000000 | AT91C_RSTC_EXTRST;
         while (1)
           ;
@@ -1006,11 +996,8 @@ int main()
     {
       unsigned short mask;
       int i;
-      // static long cnt = 0;
       /* 約5msおき */
       driver_state.cnt_updated -= 5;
-      if (driver_state.cnt_updated >= 5)
-        driver_state.cnt_updated -= 5;
 
       mask = driver_state.admask;  // analog_mask;
       if (driver_state.io_mask[0])
@@ -1108,14 +1095,18 @@ int main()
       }
     }
 
-    if (driver_state.velcontrol == 1)
+    if (driver_state.velcontrol > 0)
     {
 #define ERROR_BLINK_MS 200
       driver_state.velcontrol = 0;
-      ISR_VelocityControl();
 
       if (++mscnt >= ERROR_BLINK_MS)
       {
+        if (usb_timeout_cnt > 0)
+        {
+          printf("USB:w timeout (%d)\n\r", usb_timeout_cnt);
+          usb_timeout_cnt = 0;
+        }
         if (mscnt == ERROR_BLINK_MS &&
             driver_state.protocol_version >= 10 &&
             (motor[0].servo_level >= SERVO_LEVEL_TORQUE ||
diff --git a/tfrog-motordriver/utils.h b/tfrog-motordriver/utils.h
new file mode 100644
index 0000000..ce43b1e
--- /dev/null
+++ b/tfrog-motordriver/utils.h
@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2011-2019 T-frog Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef __UTILS_H__
+#define __UTILS_H__
+
+inline void normalize(int* val, int min, int max, int resolution)
+{
+  if (resolution <= 0)
+    return;
+  while (*val < min)
+    *val += resolution;
+  while (*val >= max)
+    *val -= resolution;
+}
+
+inline int _abs(int x)
+{
+  if (x < 0)
+    return -x;
+  return x;
+}
+
+#endif