Logger post topic

.gitignore

@@ -20,4 +20,5 @@ temp.errors
 *.ini
 .d/
 .vscode/settings.json
+.vscode/c_cpp_properties.json
 .history/

.vscode/settings.json

@@ -83,7 +83,8 @@
         "*.ipp": "cpp",
         "core": "cpp",
         "forward_list": "cpp",
-        "dense": "cpp"
+        "dense": "cpp",
+        "*.txx": "cpp"
     },
     "C_Cpp.clang_format_fallbackStyle": "Google"
 }

src/v5_hal/firmware/include/nodes/NodeManager.h

@@ -5,6 +5,7 @@
 #include "api.h"
 #include "ros_lib/ros.h"
 #include "util/Constants.h"
+#include "util/Logger.h"
 
 class Node;
 

src/v5_hal/firmware/include/nodes/sensor_nodes/InertialSensorNode.h

@@ -25,6 +25,8 @@ class InertialSensorNode : public Node {
 
     bool isAtAngle(Eigen::Rotation2Dd angle);
 
+    void reset();
+
     void teleopPeriodic();
 
     void autonPeriodic();
@@ -43,4 +45,6 @@ class InertialSensorNode : public Node {
     ros::Subscriber<v5_hal::RollPitchYaw, InertialSensorNode>* m_inertial_sensor_sub;
 
     void m_handleSensorMsg(const v5_hal::RollPitchYaw& msg);
+
+    Eigen::Rotation2Dd m_getV5Yaw();
 };

src/v5_hal/firmware/include/odometry/FollowerOdometry.h

@@ -2,10 +2,16 @@
 
 #include "odometry/Odometry.h"
 #include "util/Constants.h"
+#include "util/Logger.h"
+
+#include <math.h>
 
 class FollowerOdometry : public Odometry {
 private:
-
+    float total_x = 0;
+    float total_y = 0;
+    float turn_x = 0;
+    float turn_y = 0;
 public:
     FollowerOdometry(EncoderConfig x_encoder_config, EncoderConfig y_encoder_config, Pose current_pose=Pose());
 

src/v5_hal/firmware/include/util/Constants.h

@@ -5,6 +5,9 @@
 #define BALL_PRESENT_THRESHOLD 2700 // Threshold for Indexing Sensor to detect ball
 #define MAX_MOTOR_VOLTAGE 12000 // Max voltage value for a motor
 #define DELAY_TIME_MILLIS 5 // Standard delay time used in NodeManager
+
+// Offset to apply to get the gyro into our correct frame of reference
+// Forward is M_PI_2, so we rotate the gyro angle to match
 #define GYRO_OFFSET (M_PI_2)
 #define MAX_WHEEL_SPEED 1.1526 // Wheel speed for Holonomic Drive in m/s 
 
@@ -27,4 +30,12 @@
 #define POSE_PURSUIT_LINEAR_THRESHOLD 5.0
 #define POSE_PURSUIT_THETA_THRESHOLD 0.3
 
+#define ODOM_TRACK_WIDTH 0
+
+#define ODOM_PERPENDICULAR_X 0.8
+#define ODOM_PERPENDICULAR_Y -5.478
+
+#define ODOM_PARALLEL_X -1.75
+#define ODOM_PARALLEL_Y -3.924
+
 #endif

src/v5_hal/firmware/include/util/Logger.h

@@ -20,9 +20,15 @@ class Logger {
         WARNING,
         ERROR
     };
+
+    std::string m_message;
+    std::string m_handle_name = "logger";
+
+    static void initialize();
     static void logInfo(string message);
     static void setConsoleLoggingLevel(LoggingLevel level);
     static void giveNodeManager(NodeManager * node_manager);
+
 private:
     static LoggingLevel m_console_logging_level;
     static NodeManager * m_node_manager;

src/v5_hal/firmware/src/main.cpp

@@ -92,8 +92,8 @@ void initialize() {
 		bottom_conveyor_sensor, top_conveyor_sensor);
 
 	/* Define the odometry components */
-	x_odom_encoder = new ADIEncoderNode(node_manager, 'C', 'D', "xOdomEncoder", false);
-	y_odom_encoder = new ADIEncoderNode(node_manager, 'A', 'B', "yOdomEncoder", true);
+	x_odom_encoder = new ADIEncoderNode(node_manager, 'A', 'B', "xOdomEncoder", false);
+	y_odom_encoder = new ADIEncoderNode(node_manager, 'C', 'D', "yOdomEncoder", false);
 
 	inertial_sensor = new InertialSensorNode(node_manager, "inertialSensor", 15); // Port 15
 
@@ -104,7 +104,7 @@ void initialize() {
 	connection_checker_node = new ConnectionCheckerNode(node_manager);
 
 	/* Define autonomous components */
-	auton_manager_node = new AutonManagerNode(node_manager, holonomic_drive_node, conveyor_node, intake_node, odom_node, inertial_sensor);
+	//auton_manager_node = new AutonManagerNode(node_manager, holonomic_drive_node, conveyor_node, intake_node, odom_node, inertial_sensor);
 
 	// Call the node manager to initialize all of the nodes above
 	node_manager->initialize();

src/v5_hal/firmware/src/nodes/NodeManager.cpp

@@ -19,6 +19,7 @@ void NodeManager::initialize() {
     for (auto node_structure : m_node_structures) {
         node_structure.node->initialize();
     }
+    Logger::initialize();
 }
 
 void NodeManager::reset() {

src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp

@@ -32,7 +32,9 @@ Odometry* OdometryNode::m_getOdomClass(OdomConfig config) {
 }
 
 void OdometryNode::initialize() {
-
+    // Calibrate the gyro, and reset the orientation with the correct gyro angle
+    m_inertial_sensor_node->reset();
+	setCurrentPose(Pose(Vector2d(0, 0), m_inertial_sensor_node->getYaw()));
 }
 
 void OdometryNode::setCurrentPose(Pose pose) {
@@ -44,20 +46,15 @@ Pose OdometryNode::getCurrentPose() {
 }
 
 void OdometryNode::teleopPeriodic() {
-    Rotation2Dd current_angle(m_inertial_sensor_node->getYaw());
-
-    m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), current_angle);
-    //m_odom->Update(m_motor_1->getPosition(), m_motor_2->getPosition(), current_angle);
+    m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), m_inertial_sensor_node->getYaw());
 
     // Logger::logInfo("Robot position: " + std::to_string(m_odom->GetPose().position.x()) + " " + 
-    //                 std::to_string(m_odom->GetPose().position.y()) + " | Robot angle: " + std::to_string(m_odom->GetPose().angle.angle()));
+    //                std::to_string(m_odom->GetPose().position.y()) + " | Robot angle: " + std::to_string(m_odom->GetPose().angle.angle()));
+    //Logger::logInfo("x Encoder: " + std::to_string(m_odom_encoder_1->getValue()) + " | y Encoder: " + std::to_string(m_odom_encoder_2->getValue()));
 }
 
 void OdometryNode::autonPeriodic() {
-    Rotation2Dd current_angle(m_inertial_sensor_node->getYaw());
-
-    m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), current_angle);
-    //m_odom->Update(m_motor_1->getPosition(), m_motor_2->getPosition(), current_angle);
+    m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), m_inertial_sensor_node->getYaw());
 }
 
 OdometryNode::~OdometryNode() {

src/v5_hal/firmware/src/nodes/sensor_nodes/InertialSensorNode.cpp

@@ -2,15 +2,15 @@
 
 InertialSensorNode::InertialSensorNode(NodeManager* node_manager, 
         std::string handle_name, int sensor_port) : Node(node_manager, 20), 
-        m_yaw(0), m_gyro_offset_angle(-M_PI_2) {
+        m_yaw(0), m_gyro_offset_angle(GYRO_OFFSET) {
     m_handle_name = handle_name.insert(0, "sensor/");
     m_config = V5;
 
     m_inertial_sensor = new pros::Imu(sensor_port);
 }
 
 InertialSensorNode::InertialSensorNode(NodeManager* node_manager, std::string handle_name, 
-        std::string subscribe_handle) : Node(node_manager, 20), m_yaw(0), m_gyro_offset_angle(M_PI_2),
+        std::string subscribe_handle) : Node(node_manager, 20), m_yaw(0), m_gyro_offset_angle(GYRO_OFFSET),
         m_sub_inertial_sensor_name(subscribe_handle) {
     m_handle_name = handle_name.insert(0, "sensor/");
     m_config = ROS;
@@ -20,8 +20,15 @@ InertialSensorNode::InertialSensorNode(NodeManager* node_manager, std::string ha
 }
 
 void InertialSensorNode::m_handleSensorMsg(const v5_hal::RollPitchYaw& msg) {
+    // Convert value to radians
     Eigen::Rotation2Dd current_angle(msg.yaw  * (M_PI/180));
-    m_yaw = (current_angle.inverse()) * m_gyro_offset_angle;
+    // Rotate by the initial angle to get our current yaw
+    m_yaw = current_angle.inverse() * m_gyro_offset_angle;
+}
+
+Eigen::Rotation2Dd InertialSensorNode::m_getV5Yaw() {
+    Eigen::Rotation2Dd current_angle(m_inertial_sensor->get_yaw() * -(M_PI/180));
+    return current_angle * m_gyro_offset_angle;
 }
 
 void InertialSensorNode::initialize() {
@@ -45,29 +52,30 @@ bool InertialSensorNode::isAtAngle(Eigen::Rotation2Dd angle) {
     return fabs((m_yaw * angle.inverse()).smallestAngle()) < turning_threshold;
 }
 
+void InertialSensorNode::reset() {
+    m_inertial_sensor->reset();
+    pros::delay(5000);
+    m_yaw = m_getV5Yaw();
+}
+
 void InertialSensorNode::teleopPeriodic() {
     switch (m_config) {
         case V5:
             if (!(m_inertial_sensor->is_calibrating())) {
-                Eigen::Rotation2Dd current_angle(m_inertial_sensor->get_yaw() * -(M_PI/180));
-                m_yaw = current_angle * m_gyro_offset_angle.inverse();
+                // Convert sensor input to radians, and reverse orientation 
+                m_yaw = m_getV5Yaw();
             }
     }
-
-    // std::string msg = m_handle_name + " value: " + std::to_string(getYaw().angle());
-    // m_handle->logwarn(msg.c_str());
 }
 
 void InertialSensorNode::autonPeriodic() {
     switch (m_config) {
         case V5:
             if (!m_inertial_sensor->is_calibrating()) {
-                Eigen::Rotation2Dd current_angle(m_inertial_sensor->get_yaw() * -(M_PI/180));
-                m_yaw = current_angle * m_gyro_offset_angle.inverse();
+                // Convert sensor input to radians, and reverse orientation 
+                m_yaw = m_getV5Yaw();
             }
     }
-
-    Logger::logInfo("Robot angle: " + std::to_string(getYaw().angle()));
 }
 
 InertialSensorNode::~InertialSensorNode () {

src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp

@@ -17,35 +17,59 @@ FollowerOdometry::FollowerOdometry(EncoderConfig xEncoderConfig, EncoderConfig y
  */
 void FollowerOdometry::Update(double x_encoder_raw_ticks, double y_encoder_raw_ticks, Rotation2Dd gyro_angle) {
     // Convert the current position in ticks to a position in distance units
-    double x_dist = x_encoder_raw_ticks * Odometry::m_encoder_1_ticks_to_dist;
-    double y_dist = y_encoder_raw_ticks * Odometry::m_encoder_2_ticks_to_dist;
+    double x_encoder_dist = x_encoder_raw_ticks * Odometry::m_encoder_1_ticks_to_dist;
+    double y_encoder_dist = y_encoder_raw_ticks * Odometry::m_encoder_2_ticks_to_dist;
+
+    Vector2d x_encoder_location(ODOM_PERPENDICULAR_X, ODOM_PERPENDICULAR_Y);
+    Vector2d y_encoder_location(ODOM_PARALLEL_X, ODOM_PARALLEL_Y);
 
     // Reset the current position of the robot
     if (m_pose_reset) {
-        Odometry::m_last_encoder_1_dist = x_dist;
-        Odometry::m_last_encoder_2_dist = y_dist;
+        Odometry::m_last_encoder_1_dist = x_encoder_dist;
+        Odometry::m_last_encoder_2_dist = y_encoder_dist;
         Odometry::m_gyro_initial_angle = gyro_angle;
         Odometry::m_pose_reset = false;
     }
 
-    // Calculate the change in position since the last check
-    double x_delta = x_dist - Odometry::m_last_encoder_1_dist;
-    double y_delta = y_dist - Odometry::m_last_encoder_2_dist;
+    // Calculate the change in position of each encoder since the last check
+    double x_encoder_delta = x_encoder_dist - Odometry::m_last_encoder_1_dist;
+    double y_encoder_delta = y_encoder_dist - Odometry::m_last_encoder_2_dist;
+    Rotation2Dd angle_delta = gyro_angle * m_robot_pose.angle.inverse(); // Find change in angle
+
+    double x_delta = x_encoder_delta - (x_encoder_location.norm() * angle_delta.angle() * fabs(sin(atan(x_encoder_location.y() / x_encoder_location.x()))));
+    double y_delta = y_encoder_delta - (-1 * y_encoder_location.norm() * angle_delta.angle() * fabs(cos(atan(y_encoder_location.y() / y_encoder_location.x()))));
+
+    total_x = total_x + x_encoder_delta;
+    total_y = total_y + y_encoder_delta;
+
+    turn_x = turn_x + (x_encoder_location.norm() * angle_delta.angle() * fabs(sin(atan(x_encoder_location.y() / x_encoder_location.x()))));
+    turn_y = turn_y + (y_encoder_location.norm() * angle_delta.angle() * fabs(cos(atan(y_encoder_location.y() / y_encoder_location.x()))));
+
+    // Logger::logInfo("Encoder: " + std::to_string(x_encoder_delta) + 
+    //                 " | Modified: " + std::to_string(x_delta) + 
+    //                 " | Change in angle: " + std::to_string(angle_delta.angle()));
+
+    Logger::logInfo("Total X: " + std::to_string(total_x) + 
+                    " | Total Y: " + std::to_string(total_y) + 
+                    " | Turn X: " + std::to_string(turn_x) + 
+                    " | Turn Y: " + std::to_string(turn_y));
 
     // Convert the x and y deltas into a translation vector
-    Vector2d robot_translation(x_delta, y_delta);
+    Vector2d robot_translation(x_encoder_delta, y_encoder_delta);
 
     // Update the current angle of the robot position
-    Odometry::m_robot_pose.angle = gyro_angle * Odometry::m_gyro_initial_angle.inverse() * Rotation2Dd(m_gyro_offset);
-    //Odometry::m_robot_pose.angle = gyro_angle * Odometry::m_gyro_initial_angle.inverse() * Rotation2Dd(GYRO_OFFSET);
+    // Find the difference of the current angle to the initial angle
+    // Rotate this difference by the M_PI_2 offset to put it back in the correct frame of reference
+    Odometry::m_robot_pose.angle = gyro_angle * Odometry::m_gyro_initial_angle.inverse() * Odometry::m_gyro_offset;
 
     // Rotate the translation vector by the current angle rotation matrix
-    robot_translation = Odometry::m_robot_pose.angle * robot_translation;
+    // We need to rotate this back, as our encoders are oriented with 0 being forward
+    robot_translation = (Odometry::m_robot_pose.angle * Odometry::m_gyro_offset.inverse()) * robot_translation;
 
     // Add the current translation onto the robot position vector
     Odometry::m_robot_pose.position += robot_translation;
 
     // Update the previous values of the encoders for the next iteration
-    Odometry::m_last_encoder_1_dist = x_dist;
-    Odometry::m_last_encoder_2_dist = y_dist;
+    Odometry::m_last_encoder_1_dist = x_encoder_dist;
+    Odometry::m_last_encoder_2_dist = y_encoder_dist;
 }

src/v5_hal/firmware/src/util/Logger.cpp

@@ -3,14 +3,19 @@
 Logger::LoggingLevel Logger::m_console_logging_level;
 NodeManager* Logger::m_node_manager = nullptr;
 
+void Logger::initialize() {
+    m_publisher = new ros::Publisher(m_handle_name.c_str(), &m_message);
+    Node::m_handle->advertise(*m_publisher);
+}
+
 void Logger::giveNodeManager(NodeManager* node_manager) {
     m_node_manager = node_manager;
 }
 
 void Logger::logInfo(string message) {
     if(m_node_manager != nullptr) {
-        string msg = message;
-        m_node_manager->m_handle->logwarn(msg.c_str());
+        m_message = message;
+        m_publisher->publish(&m_message);
     }
 }