Continue on go to goal behaviour

server_v2/include/ClosedLoopMotorController.hpp

@@ -22,7 +22,7 @@ public:
 
   void set_power(uint8_t port, double power);
 
-  void set_speed(uint8_t port, double speed_ms);
+  void set_speed(uint8_t port, double speed_rad_s);
 
   void generate_step_response_data();
 

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_ms();
+  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rad_s();
 
 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_ms = {0};
+  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rad_s = {0};
 };
 
 

server_v2/include/GoToGoalController.hpp

@@ -0,0 +1,29 @@
+//
+// Created by KonstantinViesure on 26.05.22.
+//
+
+#ifndef COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
+#define COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
+
+
+class GoToGoalController {
+public:
+  static GoToGoalController &getInstance() {
+    static GoToGoalController instance;
+    return instance;
+  }
+
+  GoToGoalController(GoToGoalController const &) = delete;
+
+  void operator=(GoToGoalController const &) = delete;
+
+  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:
+  GoToGoalController() = default;
+};
+
+
+#endif //COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP

server_v2/include/OdometryController.hpp

@@ -26,6 +26,8 @@ public:
 
   Odometry get();
 
+  [[nodiscard]] bool is_enabled() const;
+
 private:
   typedef std::chrono::steady_clock clock;
 
@@ -40,6 +42,7 @@ private:
   double last_position_left{0};
   double last_position_right{0};
   std::chrono::time_point<clock> last_run;
+
 };
 
 

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 222.0
-#define ROBOT_MOTOR_SPEED_CONTROL_KI 2110.0
-#define ROBOT_MOTOR_SPEED_CONTROL_KD 2.22
-#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 0.2
+#define ROBOT_MOTOR_SPEED_CONTROL_KP 2.5
+#define ROBOT_MOTOR_SPEED_CONTROL_KI 19.0
+#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.07
+#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI
 #define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
 
 #define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
@@ -21,12 +21,15 @@
 
 #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_WHEEL_RADIUS_MM 35.5
+#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
+#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2 * 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_ARBOR_LENGTH_MM 139.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
 
 #endif //COMPLIB_SERVER_ROBOT_HPP

server_v2/include/mathUtils.hpp

@@ -2,6 +2,7 @@
 #define COMPLIB_SERVER_MATHUTILS_HPP
 
 #include <cstdint>
+#include <cmath>
 
 namespace mathUtils {
   inline int int_from_bytes(uint8_t *data, int length) {
@@ -34,6 +35,11 @@ namespace mathUtils {
       i++;
     }
   }
+
+  template<class T>
+  inline T wrap_angle_to_pi(T angle) {
+    return atan2(sin(angle), cos(angle));
+  }
 }
 
 #endif // COMPLIB_SERVER_MATHUTILS_HPP