fix : speed cmd / response confirmation OK

lib/Whill/OmniModel.h

@@ -21,7 +21,7 @@ namespace Work
 
 
             const double g = 9.80665; // gravitational acceleraation [m/s^2]
             const double g = 9.80665; // gravitational acceleraation [m/s^2]
 
 
-            const double R = 0.12; // wheel radius [m]
+            const double R = 0.125; // wheel radius [m]
             // const double W = 0.2; // wheel base / 2.0 [m] (from center to wheel)
             // const double W = 0.2; // wheel base / 2.0 [m] (from center to wheel)
             const double L = 0.435; // from center to wheel [m]
             const double L = 0.435; // from center to wheel [m]
             // const double T = 0.29; // tread / 2.0 [m] (from center to wheel)
             // const double T = 0.29; // tread / 2.0 [m] (from center to wheel)
@@ -158,8 +158,8 @@ namespace Work
 
 
             // const double& getWheelBase() const { return W; }
             // const double& getWheelBase() const { return W; }
             // const double& getTread() const { return T; }
             // const double& getTread() const { return T; }
-            double getWheelRadius() const { return R / 2.0; }
-            const double& getWheelDiameter() const { return R; }
+            double getWheelRadius() const { return R; }
+            const double& getWheelDiameter() const { return 2.0 * R; }
             const double& getWheelThickness() const { return wheel_thickness; }
             const double& getWheelThickness() const { return wheel_thickness; }
         };
         };
 
 

lib/Whill/Whill.cpp

@@ -49,6 +49,16 @@ bool WhillModelC::moveVel(float y, float x) // [m/s]
 {
 {
     float fb, lr;
     float fb, lr;
 
 
+    // Serial.print("vel cmd [m/s] (y, x) = ");
+    // Serial.print(y);
+    // Serial.print(", ");
+    // Serial.println(x);
+
+    // Serial.print("spd prf [m/s] (y, x) = ");
+    // Serial.print(spd_profile.f_m1);
+    // Serial.print(", ");
+    // Serial.println(spd_profile.t_m1);
+
     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;
 
 
@@ -171,12 +181,18 @@ void WhillModelC::parse()
 
 
                 status.info.error_code = unpacker.errorCode();
                 status.info.error_code = unpacker.errorCode();
 
 
-                // Serial.print("Wheel Spd = ");
+                // Serial.println("=================================");
+                // Serial.print("Detected Wheel Spd [km/h] = ");
                 // Serial.print(status.info.wheel.speed_l);
                 // Serial.print(status.info.wheel.speed_l);
                 // Serial.print(", ");
                 // Serial.print(", ");
                 // Serial.print(status.info.wheel.speed_r);
                 // Serial.print(status.info.wheel.speed_r);
                 // Serial.println();
                 // Serial.println();
-
+                // Serial.print("Detected Wheel Anguler Spd[rad/s] = ");
+                // Serial.print(status.info.wheel.speed_l * 1000. / 60. /60. / 0.125);
+                // Serial.print(", ");
+                // Serial.print(status.info.wheel.speed_r * 1000. / 60. /60. / 0.125);
+                // Serial.println();
+                // Serial.println("=================================");
             }
             }
             pop();
             pop();
         }
         }