adapt to new whill firmware (fb is also acc ctrl)

lib/Whill/Whill.cpp

@@ -78,14 +78,15 @@ bool WhillModelC::stop()
 
 
 bool WhillModelC::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1)
 bool WhillModelC::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1)
 {
 {
+    // WHILL New Firmware
     f_m1 = constrain(f_m1,  8,  60); // less than 60 is better
     f_m1 = constrain(f_m1,  8,  60); // less than 60 is better
     f_a1 = constrain(f_a1, 10,  90);
     f_a1 = constrain(f_a1, 10,  90);
     f_d1 = constrain(f_d1, 40, 160);
     f_d1 = constrain(f_d1, 40, 160);
     r_m1 = constrain(r_m1,  8,  60); // less than 60 is better
     r_m1 = constrain(r_m1,  8,  60); // less than 60 is better
-    r_a1 = constrain(r_a1, 10,  50);
-    r_d1 = constrain(r_d1, 40,  80);
+    r_a1 = constrain(r_a1, 10,  90);
+    r_d1 = constrain(r_d1, 40, 160);
     t_m1 = constrain(t_m1,  8,  60); // less than 60 is better
     t_m1 = constrain(t_m1,  8,  60); // less than 60 is better
-    t_a1 = constrain(t_a1, 10,  60);
+    t_a1 = constrain(t_a1, 10,  90);
     t_d1 = constrain(t_d1, 40, 160);
     t_d1 = constrain(t_d1, 40, 160);
 
 
     // save speed as [m/s]
     // save speed as [m/s]

lib/Whill/WhillOmni.cpp

@@ -74,17 +74,25 @@ bool WhillOmni::move(float x, float y, float r) // [mm/s], [mm/s], [rad/s]
     w_wheel[2] = -w_wheel[2];
     w_wheel[2] = -w_wheel[2];
 
 
     // calcurate whill's translation speed
     // calcurate whill's translation speed
-    whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
-    whill_l.x() = (w_wheel[1] - w_wheel[0]) / 1.1f * 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();
 
 
-    // true?
+    // true for previous whill (fb: jerk control, lr: acc control)
+    // whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
+    // whill_l.x() = (w_wheel[1] - w_wheel[0]) / 1.1f * 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();
+
+    // true for kinematics (not suitable for whill)
     // whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
     // 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_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.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
     // whill_r.x() = (w_wheel[3] - w_wheel[2]) / 2.f * model.getWheelRadius();
     // whill_r.x() = (w_wheel[3] - w_wheel[2]) / 2.f * model.getWheelRadius();
 
 
+    // true for whill (for new firmware)
+    whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
+    whill_l.x() = (w_wheel[1] - w_wheel[0]) / 1.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.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();

src/main.cpp

@@ -54,7 +54,7 @@ void setup()
     // WHILL setting
     // WHILL setting
     Serial.println("set speed profile");
     Serial.println("set speed profile");
 
 
-    whill.speed(SPEEDMODE::RS232C, 60, 45, 80, 60, 45, 80, 60, 60, 80);
+    whill.speed(SPEEDMODE::RS232C, 60, 90, 90, 60, 90, 90, 60, 90, 90); // for new whill firmware
     delay(1000);
     delay(1000);
 
 
     Serial.println("check speed profile");
     Serial.println("check speed profile");