Add goToDistance and goToAngle

server_v2/include/GoToController.hpp

@@ -1,28 +1,13 @@
-//
-// Created by KonstantinViesure on 26.05.22.
-//
-
 #ifndef COMPLIB_SERVER_GOTOCONTROLLER_HPP
 #define COMPLIB_SERVER_GOTOCONTROLLER_HPP
 
-
 class GoToController {
 public:
-  static GoToController &getInstance() {
-    static GoToController instance;
-    return instance;
-  }
-
-  GoToController(GoToController const &) = delete;
-
-  void operator=(GoToController const &) = delete;
 
   static void drive_distance(double distance_m, double v_ms);
 
   static void turn_degrees(double angle_deg, double v_rad_s);
 
-  static void go_to_point(double goal_x, double goal_y, double v_ms);
-
   static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s);
 
 private:

server_v2/include/Robot.hpp

@@ -19,25 +19,28 @@
 #define ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT 1.0
 #define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0
 
-#define ROBOT_ENCODER_RATE_HZ 250
+#define ROBOT_ENCODER_RATE_HZ 500
 
 #define ROBOT_WHEEL_RADIUS_MM 35.5
 #define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
 #define ROBOT_WHEEL_CIRCUMFERENCE_MM (2.0 * ROBOT_WHEEL_RADIUS_MM * M_PI)
 #define ROBOT_WHEEL_CIRCUMFERENCE_M (2 * ROBOT_WHEEL_RADIUS_M * M_PI)
-#define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
+#define ROBOT_TICKS_PER_TURN 2770.0
 #define ROBOT_ARBOR_LENGTH_MM 140.0
 #define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)
 #define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
 
 #define ROBOT_GO_TO_GOAL_K 0.99 // Between 0.5 and 1
 
+#define ROBOT_GO_TO_DISTANCE_RATE_HZ 200
 #define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0
-#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0
+#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 4.0
 #define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target
+#define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.025
 
-#define ROBOT_GO_TO_ANGLE_END_RAD (0.1 * (M_PI / 180.0)) // stop 0.1 deg before target
+#define ROBOT_GO_TO_ANGLE_RATE_HZ 200
+#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target
 #define ROBOT_GO_TO_ANGLE_ALPHA_P 0.5
-#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 3.0)
+#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 2.0)
 
 #endif //COMPLIB_SERVER_ROBOT_HPP

server_v2/protos/CompLib.proto

@@ -82,3 +82,20 @@ message OdometryReadResponse {
   double orientation = 5;
 }
 
+message DriveDistanceRequest {
+  Header header = 1;
+  double distance_m = 2;
+  double velocity_m_s = 3;
+}
+
+message TurnDegreesRequest {
+  Header header = 1;
+  double angle_degrees = 2;
+  double velocity_rad_s = 3;
+}
+
+message DriveRequest {
+  Header header = 1;
+  double linear_velocity_m_s = 2;
+  double angular_velocity_rad_s = 3;
+}

server_v2/src/ClosedLoopMotorController.cpp

@@ -152,15 +152,13 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
 
 void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks_per_turn) {
   auto k_p = 0.0001;
-  auto power = 40.0;
+  auto power = 25.0;
   auto min_power = 7.5;
-  auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
+  auto port = ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT;
 
   auto ticks = 0.0;
   auto target_ticks = ticks_per_turn * turns;
   while (ticks != target_ticks) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(5));
-
     auto power_mult = fmax(-1.0, fmin(1.0, (target_ticks - ticks) * k_p));
     auto adjusted_power = power * power_mult;
     if (adjusted_power < 0 && adjusted_power > -min_power) {
@@ -171,9 +169,11 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks
 
     set_power(port, adjusted_power);
 
-    ticks = Encoders::getInstance().get_positions().at(port);
     spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};",
                   ticks, target_ticks, adjusted_power);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    ticks = Encoders::getInstance().get_positions().at(port);
   }
 
   set_power(port, 0);

server_v2/src/GoToController.cpp

@@ -19,83 +19,45 @@ void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s)
   ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s);
 }
 
-void GoToController::go_to_point(double goal_x, double goal_y, double v_ms) {
+// +dist, +vms -> forward
-  auto was_enabled = OdometryController::getInstance().is_enabled();
+// -dist, +vms -> backward
-  OdometryController::getInstance().enable();
+// +dist, -vms -> backward
-  auto start_pos = OdometryController::getInstance().get();
+// -dist, -vms -> backward
-
-  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
-
-  auto abs_distance = fabs(sqrt(
-      pow(goal_x - start_pos.get_x_position(), 2) + pow(goal_y - start_pos.get_y_position(), 2)
-  ));
-  while (abs_distance > 0.05) {
-    auto current_pos = OdometryController::getInstance().get();
-    auto alpha = mathUtils::wrap_angle_to_pi(
-        (atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()) - (current_pos.get_angular_orientation() + M_PI_2))
-    );
-
-    auto vr_ms = v_ms * (cos(alpha) - ROBOT_GO_TO_GOAL_K * sin(alpha));
-    auto vl_ms = v_ms * (cos(alpha) + ROBOT_GO_TO_GOAL_K * sin(alpha));
-    auto vr_rad_s = vr_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
-    auto vl_rad_s = vl_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
-
-    ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s);
-    ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s);
-
-    abs_distance = fabs(sqrt(
-        pow(goal_x - current_pos.get_x_position(), 2) + pow(goal_y - current_pos.get_y_position(), 2)
-    ));
-    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}",
-                  current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
-                  goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position(),
-                  current_pos.get_angular_orientation() + M_PI_2, atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()), alpha,
-                  vr_rad_s, vl_rad_s);
-
-    std::this_thread::sleep_for(sleep_duration);
-  }
-
-  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
-  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
-
-  if (!was_enabled) {
-    OdometryController::getInstance().disable();
-  }
-}
-
 void GoToController::drive_distance(double distance_m, double v_m_s) {
+  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_DISTANCE_RATE_HZ);
+
   if (distance_m < 0 && v_m_s > 0) {
-    distance_m = fabs(distance_m);
     v_m_s *= -1;
   }
 
   auto was_enabled = OdometryController::getInstance().is_enabled();
   OdometryController::getInstance().enable();
-  auto start_pos = OdometryController::getInstance().get();
+  auto start_x = OdometryController::getInstance().get().get_x_position();
+  auto end_x = start_x + distance_m;
 
-  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
+  auto distance_to_target = distance_m;
-
+  while (fabs(distance_to_target) > ROBOT_GO_TO_DISTANCE_END_M) {
-  auto abs_distance_to_target = distance_m;
-  while (abs_distance_to_target > ROBOT_GO_TO_DISTANCE_END_M) {
     std::this_thread::sleep_for(sleep_duration);
-
     auto current_pos = OdometryController::getInstance().get();
-
+    distance_to_target = end_x - current_pos.get_x_position();
-    auto distance_driven = fabs(current_pos.get_x_position() - start_pos.get_x_position());
-    abs_distance_to_target = fabs(distance_m - distance_driven);
-
     auto alpha = -mathUtils::wrap_angle_to_pi(current_pos.get_angular_orientation());
 
-    auto vel_mult = fmax(0, fmin(1, abs_distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P));
+    auto vel_mult = mathUtils::limit_max(distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P, -1.0, 1.0);
-    diff_drive_inverse_kinematics(v_m_s * vel_mult, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P);
+    auto adjusted_speed = mathUtils::limit_min(fabs(v_m_s) * vel_mult, ROBOT_GO_TO_DISTANCE_MIN_VEL);
+    diff_drive_inverse_kinematics(adjusted_speed, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P);
 
-    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}",
+    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};",
                   current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
-                  distance_driven, abs_distance_to_target,
+                  distance_to_target,
-                  alpha, vel_mult
+                  alpha, vel_mult, adjusted_speed
     );
   }
 
+  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
+  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
   ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
   ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
 
@@ -104,8 +66,13 @@ void GoToController::drive_distance(double distance_m, double v_m_s) {
   }
 }
 
+// +ang, +rad -> cw
+// -ang, +rad -> ccw
+// +ang, -rad -> ccw
+// -ang, -rad -> ccw
 void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
   auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0));
+  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_ANGLE_RATE_HZ);
 
   if (angle_rad < 0 && v_rad_s > 0) {
     v_rad_s *= -1;
@@ -113,28 +80,24 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
 
   auto was_enabled = OdometryController::getInstance().is_enabled();
   OdometryController::getInstance().enable();
-  auto start_pos = OdometryController::getInstance().get();
 
-  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
+  auto start_angle = OdometryController::getInstance().get().get_angular_orientation();
-
+  auto end_angle = mathUtils::wrap_angle_to_pi(start_angle + angle_rad);
-  auto angle_to_target = angle_rad;
+  auto angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - start_angle);
-  auto angle_turned = 0.0;
+  while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) {
-  while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD && angle_turned < angle_rad) {
     std::this_thread::sleep_for(sleep_duration);
+    auto current_angle = OdometryController::getInstance().get().get_angular_orientation();
+    angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - current_angle);
 
-    auto current_pos = OdometryController::getInstance().get();
+    if (fabs(angle_to_target) > M_PI_2) {
-
+      diff_drive_inverse_kinematics(0.0, v_rad_s);
-    angle_turned = current_pos.get_angular_orientation() - start_pos.get_angular_orientation();
+      spdlog::debug("{:03.4f}; {:03.4f};", current_angle, angle_to_target);
-    angle_to_target = mathUtils::wrap_angle_to_pi(angle_rad - angle_turned);
+    } else {
-
+      auto speed_mult = mathUtils::limit_max(angle_to_target * ROBOT_GO_TO_ANGLE_ALPHA_P, -1.0, 1.0);
-    auto speed_mult = mathUtils::limit_max(fabs(angle_to_target) * ROBOT_GO_TO_ANGLE_ALPHA_P, 0.0, 1.0);
+      auto adjusted_speed = mathUtils::limit_min(fabs(v_rad_s) * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL);
-    auto adjusted_speed = mathUtils::limit_min(v_rad_s * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL);
       diff_drive_inverse_kinematics(0.0, adjusted_speed);
-
+      spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed);
-    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};",
+    }
-                  current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
-                  angle_turned, angle_to_target, adjusted_speed
-    );
   }
 
   ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);

server_v2/src/OdometryController.cpp

@@ -80,7 +80,7 @@ OdometryController::OdometryController() {
       }
       current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation);
 
-      spdlog::info("{} {} {} {} {} {}",
+      spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
                    current_position_left, current_position_right,
                    distance_left, distance_right,
                    distance, theta);

server_v2/src/communication/MessageProcessor.cpp

@@ -6,6 +6,7 @@
 #include "include/Encoders.hpp"
 #include "include/IRSensors.hpp"
 #include "include/ClosedLoopMotorController.hpp"
+#include "include/GoToController.hpp"
 
 using namespace CompLib;
 
@@ -46,6 +47,24 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
       ClosedLoopMotorController::getInstance().set_speed(request.port(), request.speed());
       return MessageBuilder::default_successful_generic_response();
     }
+
+    // Drive
+    if (messageTypeName == DriveDistanceRequest::GetDescriptor()->full_name()) {
+      DriveDistanceRequest request;
+      request.ParseFromString(serializedMessage);
+      GoToController::drive_distance(request.distance_m(), request.velocity_m_s());
+      return MessageBuilder::default_successful_generic_response();
+    } else if (messageTypeName == TurnDegreesRequest::GetDescriptor()->full_name()) {
+      TurnDegreesRequest request;
+      request.ParseFromString(serializedMessage);
+      GoToController::turn_degrees(request.angle_degrees(), request.velocity_rad_s());
+      return MessageBuilder::default_successful_generic_response();
+    } else if (messageTypeName == DriveRequest::GetDescriptor()->full_name()) {
+      DriveRequest request;
+      request.ParseFromString(serializedMessage);
+      GoToController::diff_drive_inverse_kinematics(request.linear_velocity_m_s(), request.angular_velocity_rad_s());
+      return MessageBuilder::default_successful_generic_response();
+    }
   } catch (const std::exception &ex) {
     spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what());
     google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what());

server_v2/src/main.cpp

@@ -7,6 +7,7 @@
 #include "include/communication/UnixSocketServer.hpp"
 #include "include/communication/TCPSocketServer.hpp"
 #include "include/GoToController.hpp"
+#include "include/Encoders.hpp"
 
 using namespace std;
 
@@ -38,7 +39,16 @@ int main(int argc, char *argv[]) {
 
 //  GoToGoalController::go_to_point(2, 0, 0.2);
 //  GoToController::drive_distance(-4, 0.2);
-  GoToController::turn_degrees(180, M_PI);
+//  GoToController::turn_degrees(90, M_PI);
+//  while (true) {
+//    this_thread::sleep_for(std::chrono::milliseconds(10));
+//    ClosedLoopMotorController::getInstance().set_power(0, 100);
+//    spdlog::debug("{}", Encoders::getInstance().get_velocities_rad_s().at(0));
+//  }
+
+//  GoToController::drive_distance(2, 0.3);
+//  std::this_thread::sleep_for(std::chrono::seconds(2));
+//  GoToController::drive_distance(-2, 0.3);
 
 //  GoToController::drive_distance(2, 0.2);
 //  GoToController::turn_degrees(-90, M_PI_2);
@@ -52,7 +62,7 @@ int main(int argc, char *argv[]) {
 //  GoToController::drive_distance(2, 0.2);
 //  GoToController::turn_degrees(-90, M_PI_2);
 
-//  ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1]));
+//  ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(10, stoi(argv[1]));
 
 //  ClosedLoopMotorController::getInstance().generate_step_response_data();
 //  ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();