Change RPM to M/S

server_v2/include/ClosedLoopMotorController.hpp

@@ -22,7 +22,11 @@ public:
 
   void set_power(uint8_t port, double power);
 
-  void set_speed(uint8_t port, double speed_rpm);
+  void set_speed(uint8_t port, double speed_ms);
+
+  void generate_step_response_data();
+
+  void generate_tuned_step_response_data();
 
 private:
   enum ControlMode : uint8_t {

server_v2/include/Encoders.hpp

@@ -19,7 +19,7 @@ public:
 
   std::array<int32_t, ROBOT_MOTOR_COUNT> get_positions();
 
-  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rpm();
+  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ms();
 
 private:
   Encoders() = default;
@@ -28,7 +28,7 @@ private:
   Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
 
   std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
-  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rpm = {0};
+  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ms = {0};
 };
 
 

server_v2/include/Robot.hpp

@@ -7,10 +7,10 @@
 #define ROBOT_IR_RATE_HZ 250
 
 #define ROBOT_MOTOR_COUNT 4
-#define ROBOT_MOTOR_SPEED_CONTROL_KP 0.5
+#define ROBOT_MOTOR_SPEED_CONTROL_KP 222.0
-#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
+#define ROBOT_MOTOR_SPEED_CONTROL_KI 2110.0
-#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.025
+#define ROBOT_MOTOR_SPEED_CONTROL_KD 2.22
-#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 100.0
+#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 0.2
 #define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
 
 #define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
@@ -22,6 +22,7 @@
 #define ROBOT_ENCODER_RATE_HZ 250
 
 #define ROBOT_WHEEL_CIRCUMFERENCE_MM (71.0 * M_PI)
+#define ROBOT_WHEEL_CIRCUMFERENCE_M (ROBOT_WHEEL_CIRCUMFERENCE_MM / 1000.0)
 #define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
 #define ROBOT_ARBOR_LENGTH_MM 139.0
 #define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)

server_v2/src/ClosedLoopMotorController.cpp

@@ -1,4 +1,8 @@
 #include <algorithm>
+
+#include <spdlog/spdlog.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+
 #include "include/ClosedLoopMotorController.hpp"
 #include "include/Motors.hpp"
 #include "include/Encoders.hpp"
@@ -11,8 +15,8 @@ void ClosedLoopMotorController::set_power(uint8_t port, double power) {
   Motors::set_power(port, power);
 }
 
-void ClosedLoopMotorController::set_speed(uint8_t port, double speed_rpm) {
+void ClosedLoopMotorController::set_speed(uint8_t port, double speed_ms) {
-  speed_targets.at(port) = speed_rpm;
+  speed_targets.at(port) = speed_ms;
   if (control_modes.at(port) != ControlMode::SPEED) {
     pids.at(port) = PID(
         ROBOT_MOTOR_SPEED_CONTROL_KP, ROBOT_MOTOR_SPEED_CONTROL_KI, ROBOT_MOTOR_SPEED_CONTROL_KD,
@@ -36,14 +40,98 @@ ClosedLoopMotorController::ClosedLoopMotorController() {
       continue;
     }
 
-    auto speeds = Encoders::getInstance().get_velocities_rpm();
+    auto speeds_ms = Encoders::getInstance().get_velocities_ms();
     for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
       if (control_modes.at(i) == ControlMode::SPEED) {
-        double power = pids.at(i)(speed_targets.at(i), speeds.at(i));
+        double power = pids.at(i)(speed_targets.at(i), speeds_ms.at(i));
         Motors::set_power(i, power);
       }
     }
   }
 }
 
+void ClosedLoopMotorController::generate_step_response_data() {
+  auto console = spdlog::stdout_color_mt("generate_step_response_data");
+  console->set_pattern("%v");
 
+  auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
+  auto input = 25.0;
+
+  speed_targets.at(port) = input;
+  pids.at(port) = PID(
+      1.0, 0.0, 0.0,
+      1000.0, 100.0);
+  control_modes.at(port) = ControlMode::SPEED;
+
+  std::this_thread::sleep_for(std::chrono::seconds(2));
+
+  auto start_time = std::chrono::steady_clock::now();
+
+  for (int i = 0; i < 500; ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
+    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
+  }
+
+  input = 50.0;
+  speed_targets.at(port) = input;
+  for (int i = 0; i < 500; ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
+    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
+  }
+
+  speed_targets.at(port) = 0.0;
+  control_modes.at(port) = ControlMode::NONE;
+}
+
+void ClosedLoopMotorController::generate_tuned_step_response_data() {
+  auto console = spdlog::stdout_color_mt("generate_tuned_step_response_data");
+  console->set_pattern("%v");
+
+  auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
+  auto input = 0.25;
+
+  speed_targets.at(port) = input;
+  pids.at(port) = PID(
+      221.0, 2110.0, 2.21,
+      0.2, 100.0);
+  control_modes.at(port) = ControlMode::SPEED;
+
+  std::this_thread::sleep_for(std::chrono::seconds(2));
+
+  auto start_time = std::chrono::steady_clock::now();
+
+  for (int i = 0; i < 500; ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
+    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
+  }
+
+  input = 0.35;
+  speed_targets.at(port) = input;
+  for (int i = 0; i < 500; ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
+    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
+  }
+
+  input = 0.1;
+  speed_targets.at(port) = input;
+  for (int i = 0; i < 500; ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
+    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
+
+    input += 0.001;
+    speed_targets.at(port) = input;
+  }
+
+  speed_targets.at(port) = 0.0;
+  control_modes.at(port) = ControlMode::NONE;
+}

server_v2/src/Encoders.cpp

@@ -15,15 +15,16 @@ std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
   return cached_positions;
 }
 
-std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rpm() {
+std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ms() {
   if (velocity_cache.is_expired()) {
     uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
     Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
     for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
       auto velocity_tps = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
-      cached_velocities_rpm.at(i) = velocity_tps * 60.0 / ROBOT_TICKS_PER_TURN;
+      auto velocity_rps = velocity_tps / ROBOT_TICKS_PER_TURN;
+      cached_velocities_ms.at(i) = velocity_rps * ROBOT_WHEEL_CIRCUMFERENCE_M;
     }
     velocity_cache.update();
   }
-  return cached_velocities_rpm;
+  return cached_velocities_ms;
 }

server_v2/src/communication/MessageProcessor.cpp

@@ -20,7 +20,7 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
     if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) {
       return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions());
     } else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) {
-      return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rpm());
+      return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_ms());
     }
 
     // IR Sensors

server_v2/src/main.cpp

@@ -26,6 +26,8 @@ int main() {
 //    spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
 //    usleep(1000);
 //  }
+
+  ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
   std::this_thread::sleep_for(std::chrono::hours(12));
   return 0;
 }