Update CMakeLists.txt to copy Phoenix libraries

CMakeLists.txt

@@ -132,15 +132,15 @@ include_directories(
 # add_library(${PROJECT_NAME}
 # src/${PROJECT_NAME}/wrevolution.cpp
 # )
-function(MakeTalonTarget TARGET_NAME)
-    if (${CMAKE_LIBRARY_ARCHITECTURE} STREQUAL "arm-linux-gnueabihf") # Jetson TK1 / Pi
-        set(PHOENIX_LIBS_DIR ${PROJECT_SOURCE_DIR}/lib/raspberry)
-    elseif (${CMAKE_LIBRARY_ARCHITECTURE} STREQUAL "aarch64-linux-gnu") # Jetson TX2
-        set(PHOENIX_LIBS_DIR ${PROJECT_SOURCE_DIR}/lib/jetsontx)
-    else()
-        set(PHOENIX_LIBS_DIR ${PROJECT_SOURCE_DIR}/lib/x86-64)
-    endif()
+if(${CMAKE_LIBRARY_ARCHITECTURE} STREQUAL "arm-linux-gnueabihf") # Jetson TK1 / Pi
+    set(PHOENIX_LIBS_DIR ${PROJECT_SOURCE_DIR}/lib/raspberry)
+elseif(${CMAKE_LIBRARY_ARCHITECTURE} STREQUAL "aarch64-linux-gnu") # Jetson TX2
+    set(PHOENIX_LIBS_DIR ${PROJECT_SOURCE_DIR}/lib/jetsontx)
+else()
+    set(PHOENIX_LIBS_DIR ${PROJECT_SOURCE_DIR}/lib/x86-64)
+endif()
 
+function(MakeTalonTarget TARGET_NAME)
     target_link_libraries(${TARGET_NAME} PRIVATE ${PHOENIX_LIBS_DIR}/libCTRE_Phoenix.so)
     target_link_libraries(${TARGET_NAME} PRIVATE ${PHOENIX_LIBS_DIR}/libCTRE_PhoenixCCI.so)
 
@@ -153,3 +153,13 @@ function(MakeROSTarget TARGET_NAME)
 endfunction(MakeROSTarget)
 
 add_subdirectory(src)
+
+# Copy Phoenix shared libraries
+file(
+    COPY ${PHOENIX_LIBS_DIR}/libCTRE_Phoenix.so
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/lib
+)
+file(
+    COPY ${PHOENIX_LIBS_DIR}/libCTRE_PhoenixCCI.so
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/lib
+)

src/app/Pigeon2/main.cpp

@@ -2,25 +2,37 @@
 #include "ros/node_handle.h"
 #include "ros/rate.h"
 #include "std_msgs/Float64.h"
+#include "std_srvs/Empty.h"
+#include <cmath>
 #define Phoenix_No_WPI
-#include "ctre/phoenix/sensors/Pigeon2.h"
 #include "ctre/phoenix/platform/can/PlatformCAN.h"
+#include "ctre/phoenix/sensors/Pigeon2.h"
 
 auto main(int32_t argc, char **argv) -> int32_t {
 
     ctre::phoenix::platform::can::PlatformCAN::SetCANInterface("can0");
-    ctre::phoenix::sensors::Pigeon2 pigeon = ctre::phoenix::sensors::Pigeon2(1);
+    ctre::phoenix::sensors::Pigeon2 pigeon = ctre::phoenix::sensors::Pigeon2(10);
+    pigeon.SetYaw(0);
+
+    // enable magnetometer
+    // pigeon.ConfigEnableCompass(true);
+    // pigeon.SetYawToCompass();
 
     ros::init(argc, argv, "PigeonIMU");
     ros::NodeHandle nHandle;
 
     auto headingPub{nHandle.advertise<std_msgs::Float64>("/heading", 10)};
+    auto resetHeading{nHandle.advertiseService<std_srvs::Empty>("reset_heading", [&pigeon]() {
+        int err = pigeon.SetYaw(0);
+        return err == 0;
+    })};
 
     ros::Rate rate(10);
 
     while (ros::ok()) {
         std_msgs::Float64 headingMsg;
-        headingMsg.data = pigeon.GetYaw();
+        // Pigeon returns accumulated heading
+        headingMsg.data = std::fmod(pigeon.GetYaw(), 360);
 
         headingPub.publish(headingMsg);
 

src/app/RosControlTest/test_sinusoidal_power.py

@@ -1,14 +1,15 @@
 #!/usr/bin/env python3
 import rospy
 import std_msgs
+
 rospy.init_node("temp_node")
 import math
+
 pub = rospy.Publisher("/MOTOR_1/power", std_msgs.msg.Float64, queue_size=10)
 rate = rospy.Rate(20)
 t = 0
 while not rospy.is_shutdown():
-    p = math.sin(2*math.pi*t/(20*10))
+    p = math.sin(2 * math.pi * t / (20 * 10))
     t += 1
     pub.publish(p)
     rate.sleep()
-