add debug comment

lib/Whill/Whill.cpp

@@ -52,6 +52,11 @@ bool WhillModelC::moveVel(float y, float x) // [m/s]
     fb = y / spd_profile.f_m1 * 100.f;
     fb = y / spd_profile.f_m1 * 100.f;
     lr = x / spd_profile.t_m1 * 100.f;
     lr = x / spd_profile.t_m1 * 100.f;
 
 
+    // Serial.println("joystick");
+    // Serial.print(fb);
+    // Serial.print(", ");
+    // Serial.println(lr);
+
     if (abs(fb) > 100)
     if (abs(fb) > 100)
     {
     {
         Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(fb);
         Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(fb);
@@ -164,6 +169,13 @@ void WhillModelC::parse()
                 status.info.wheel.speed_l = unpacker.speedL();
                 status.info.wheel.speed_l = unpacker.speedL();
 
 
                 status.info.error_code = unpacker.errorCode();
                 status.info.error_code = unpacker.errorCode();
+
+                // Serial.print("Wheel Spd = ");
+                // Serial.print(status.info.wheel.speed_l);
+                // Serial.print(", ");
+                // Serial.print(status.info.wheel.speed_r);
+                // Serial.println();
+
             }
             }
             pop();
             pop();
         }
         }

lib/Whill/WhillOmni.cpp

@@ -79,6 +79,12 @@ bool WhillOmni::move(float x, float y, float r) // [mm/s], [mm/s], [rad/s]
     whill_r.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
     whill_r.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
     whill_r.x() = (w_wheel[3] - w_wheel[2]) / 1.1f * model.getWheelRadius();
     whill_r.x() = (w_wheel[3] - w_wheel[2]) / 1.1f * model.getWheelRadius();
 
 
+    // true?
+    // whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
+    // whill_l.x() = (w_wheel[1] - w_wheel[0]) / 2.f * model.getWheelRadius();
+    // whill_r.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
+    // whill_r.x() = (w_wheel[3] - w_wheel[2]) / 2.f * model.getWheelRadius();
+
     // convert whill direction
     // convert whill direction
     whill_l.x() = -whill_l.x();
     whill_l.x() = -whill_l.x();
     whill_r.x() = -whill_r.x();
     whill_r.x() = -whill_r.x();