fix packet recv err and set fps w/ whill comm

include/global.h

@@ -10,7 +10,7 @@
 
 
 static constexpr uint32_t WHILL_SERIAL_BAUD = 38400;
 static constexpr uint32_t WHILL_SERIAL_BAUD = 38400;
 static constexpr uint32_t XBEE_SERIAL_BAUD = 38400;
 static constexpr uint32_t XBEE_SERIAL_BAUD = 38400;
-static constexpr uint16_t WHILL_INFO_INTERVAL_MS = 100;
+static constexpr uint16_t WHILL_INFO_INTERVAL_MS = 500;
 
 
 static constexpr double XBEE_COMMAND_TIMEOUT_MS = 1000.;
 static constexpr double XBEE_COMMAND_TIMEOUT_MS = 1000.;
 
 

include/main.h

@@ -21,6 +21,7 @@ MPU9250 mpu;
 PCA9624 irled(0x60);
 PCA9624 irled(0x60);
 RzmEepromMap eepmap;
 RzmEepromMap eepmap;
 StopWatch stopwatch;
 StopWatch stopwatch;
+FrameRateCounter fps(30);
 
 
 void printSpeedProfile(WhillOmni::LR lr)
 void printSpeedProfile(WhillOmni::LR lr)
 {
 {
@@ -254,14 +255,17 @@ void setVelocity()
             vel[0] = unpacker.velocityX();
             vel[0] = unpacker.velocityX();
             vel[1] = unpacker.velocityY();
             vel[1] = unpacker.velocityY();
             vel[2] = unpacker.velocityW();
             vel[2] = unpacker.velocityW();
-            unpacker.pop();
             stopwatch.restart();
             stopwatch.restart();
         }
         }
+        unpacker.pop();
     }
     }
 
 
     if (stopwatch.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS))
     if (stopwatch.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS))
     {
     {
-        if (b_motor_active) whill.move(vel[0], vel[1], vel[2]);
+        if (b_motor_active)
+        {
+            if (fps.isNextFrame()) whill.move(vel[0], vel[1], vel[2]);
+        }
     }
     }
 }
 }
 
 
@@ -269,7 +273,6 @@ void sendStatus()
 {
 {
     if (b_streaming)
     if (b_streaming)
     {
     {
-        Serial.println(millis());
         packer.pack(HARDWARE_ID, whill.battery(WhillOmni::LR::L), whill.battery(WhillOmni::LR::R), whill.errorCode(WhillOmni::LR::L), whill.errorCode(WhillOmni::LR::R));
         packer.pack(HARDWARE_ID, whill.battery(WhillOmni::LR::L), whill.battery(WhillOmni::LR::R), whill.errorCode(WhillOmni::LR::L), whill.errorCode(WhillOmni::LR::R));
         XBEE_SERIAL.write(packer.data(), packer.size());
         XBEE_SERIAL.write(packer.data(), packer.size());
     }
     }

lib/Whill/WhillOmni.cpp

@@ -75,9 +75,9 @@ bool WhillOmni::move(float x, float y, float r) // [mm/s], [mm/s], [rad/s]
 
 
     // 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.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
-    whill_l.x() = (w_wheel[1] - w_wheel[0]) / 0.5f * 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.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
-    whill_r.x() = (w_wheel[3] - w_wheel[2]) / 0.5f * 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();

src/main.cpp

@@ -71,6 +71,7 @@ void setup()
     attachInterrupt(digitalPinToInterrupt(PIN_EMERGENCY), disableMotor, FALLING);
     attachInterrupt(digitalPinToInterrupt(PIN_EMERGENCY), disableMotor, FALLING);
 
 
     Serial.println("start program");
     Serial.println("start program");
+    fps.start();
 }
 }