Pigeon IMU

.vscode/c_cpp_properties.json

@@ -6,9 +6,7 @@
         "limitSymbolsToIncludedHeaders": false
       },
       "includePath": [
-        "/home/danielz/WRover_Software/devel/include/**",
         "/opt/ros/noetic/include/**",
-        "/home/danielz/catkin_ws/src/wrevolution/include/**",
         "/usr/include/**"
       ],
       "name": "ROS",
@@ -19,4 +17,4 @@
     }
   ],
   "version": 4
-}
+}

CMakeLists.txt

@@ -63,6 +63,7 @@ add_service_files(
     ResetSettings.srv
     ResetEncoder.srv
     Reverse.srv
+    SetHeading.srv
     ZeroPowerBehavior.srv
 )
 
@@ -132,29 +133,17 @@ include_directories(
 # add_library(${PROJECT_NAME}
 # src/${PROJECT_NAME}/wrevolution.cpp
 # )
+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)
-    if(${CMAKE_LIBRARY_ARCHITECTURE} STREQUAL "arm-linux-gnueabihf") # Jetson TK1 / Pi
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/raspberry/libCTRE_Phoenix.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/raspberry/libCTRE_PhoenixCCI.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/raspberry/libCTRE_PhoenixDiagnostics.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/raspberry/libCTRE_PhoenixPlatform_socketcan.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/raspberry/libCTRE_PhoenixCanutils.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/raspberry/libCTRE_PhoenixCore.a)
-    elseif(${CMAKE_LIBRARY_ARCHITECTURE} STREQUAL "aarch64-linux-gnu") # Jetson TX2
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/jetsontx/libCTRE_Phoenix.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/jetsontx/libCTRE_PhoenixCCI.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/jetsontx/libCTRE_PhoenixDiagnostics.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/jetsontx/libCTRE_PhoenixPlatform_socketcan.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/jetsontx/libCTRE_PhoenixCanutils.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/jetsontx/libCTRE_PhoenixCore.a)
-    else()
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/x86-64/libCTRE_Phoenix.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/x86-64/libCTRE_PhoenixCCI.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/x86-64/libCTRE_PhoenixDiagnostics.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/x86-64/libCTRE_PhoenixPlatform_socketcan.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/x86-64/libCTRE_PhoenixCanutils.a)
-        target_link_libraries(${TARGET_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/lib/x86-64/libCTRE_PhoenixCore.a)
-    endif()
+    target_link_libraries(${TARGET_NAME} PRIVATE ${PHOENIX_LIBS_DIR}/libCTRE_Phoenix.so)
+    target_link_libraries(${TARGET_NAME} PRIVATE ${PHOENIX_LIBS_DIR}/libCTRE_PhoenixCCI.so)
 
     target_link_libraries(${TARGET_NAME} PRIVATE Threads::Threads)
 endfunction(MakeTalonTarget)
@@ -164,4 +153,14 @@ function(MakeROSTarget TARGET_NAME)
     add_dependencies(${TARGET_NAME} ${catkin_EXPORTED_TARGETS})
 endfunction(MakeROSTarget)
 
-add_subdirectory(src)
+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
+)

include/ctre/Phoenix.h

@@ -14,6 +14,7 @@
 #include "ctre/phoenix/paramEnum.h"
 #include "ctre/phoenix/HsvToRgb.h"
 #include "ctre/phoenix/LinearInterpolation.h"
+#include "ctre/phoenix/led/CANdle.h"
 #include "ctre/phoenix/motion/BufferedTrajectoryPointStream.h"
 #include "ctre/phoenix/motion/MotionProfileStatus.h"
 #include "ctre/phoenix/motion/TrajectoryPoint.h"
@@ -30,6 +31,7 @@
 #include "ctre/phoenix/music/Orchestra.h"
 #include "ctre/phoenix/sensors/CANCoder.h"
 #include "ctre/phoenix/sensors/PigeonIMU.h"
+#include "ctre/phoenix/sensors/Pigeon2.h"
 #include "ctre/phoenix/signals/MovingAverage.h"
 #include "ctre/phoenix/tasking/Schedulers/ConcurrentScheduler.h"
 #include "ctre/phoenix/tasking/ILoopable.h"
@@ -40,6 +42,9 @@
 #include "ctre/phoenix/motorcontrol/can/WPI_TalonFX.h"
 #include "ctre/phoenix/motorcontrol/can/WPI_TalonSRX.h"
 #include "ctre/phoenix/motorcontrol/can/WPI_VictorSPX.h"
+#include "ctre/phoenix/sensors/WPI_CANCoder.h"
+#include "ctre/phoenix/sensors/WPI_PigeonIMU.h"
+#include "ctre/phoenix/sensors/WPI_Pigeon2.h"
 #include "ctre/phoenix/tasking/ButtonMonitor.h"
 #endif
 
@@ -56,6 +61,7 @@
  */
 using namespace ctre;
 using namespace ctre::phoenix;
+using namespace ctre::phoenix::led;
 using namespace ctre::phoenix::motion;
 using namespace ctre::phoenix::motorcontrol;
 using namespace ctre::phoenix::motorcontrol::can;

include/ctre/phoenix/CANifier.h

@@ -9,6 +9,7 @@
 #include "ctre/phoenix/CANifierStatusFrame.h"
 #include "ctre/phoenix/CANifierStickyFaults.h"
 #include "ctre/phoenix/CANifierFaults.h"
+#include "ctre/phoenix/sensors/SensorVelocityMeasPeriod.h"
 #include "ctre/phoenix/CANifierVelocityMeasPeriod.h"
 
 namespace ctre {namespace phoenix {
@@ -20,7 +21,7 @@ struct CANifierConfiguration : CustomParamConfiguration{
     /** 
      * Velocity measurement period to use
      */
-    CANifierVelocityMeasPeriod velocityMeasurementPeriod;
+    ctre::phoenix::sensors::SensorVelocityMeasPeriod velocityMeasurementPeriod;
     /**
      * Velocity measurement window to use
      */
@@ -39,7 +40,7 @@ struct CANifierConfiguration : CustomParamConfiguration{
     bool clearPositionOnQuadIdx;
 
 	CANifierConfiguration() : 
-		velocityMeasurementPeriod(Period_100Ms), 
+		velocityMeasurementPeriod(ctre::phoenix::sensors::SensorVelocityMeasPeriod::Period_100Ms), 
 		velocityMeasurementWindow(64), 	
 	    clearPositionOnLimitF(false),
         clearPositionOnLimitR(false),
@@ -61,7 +62,7 @@ struct CANifierConfiguration : CustomParamConfiguration{
      */
     std::string toString(std::string prependString) {
 
-        std::string retstr = prependString + ".velocityMeasurementPeriod = " + CANifierVelocityMeasPeriodRoutines::toString(velocityMeasurementPeriod) + ";\n";
+        std::string retstr = prependString + ".velocityMeasurementPeriod = " + ctre::phoenix::sensors::SensorVelocityMeasPeriodRoutines::toString(velocityMeasurementPeriod) + ";\n";
         retstr += prependString + ".velocityMeasurementWindow = " + std::to_string(velocityMeasurementWindow) + ";\n";
         retstr += prependString + ".clearPositionOnLimitF = " + std::to_string(clearPositionOnLimitF) + ";\n";
         retstr += prependString + ".clearPositionOnLimitR = " + std::to_string(clearPositionOnLimitR) + ";\n";
@@ -325,6 +326,10 @@ class CANifier: public CANBusAddressable {
 	 *            If zero, no blocking or checking is performed.
 	 * @return Error Code generated by function. 0 indicates no error.
 	 */
+	ErrorCode ConfigVelocityMeasurementPeriod(
+			ctre::phoenix::sensors::SensorVelocityMeasPeriod period, int timeoutMs = 0);
+
+	[[deprecated("Use the overload with SensorVelocityMeasPeriod instead")]]
 	ErrorCode ConfigVelocityMeasurementPeriod(
 			CANifierVelocityMeasPeriod period, int timeoutMs = 0);
 	/**

include/ctre/phoenix/ErrorCode.h

@@ -1,13 +1,16 @@
 #pragma once
+
 #include <stdint.h>
 
 namespace ctre {
 namespace phoenix {
 
 enum ErrorCode
-: int32_t
+#ifdef __cplusplus
+	: int32_t
+#endif
 {
-	OK = 0, 
+	OK = 0,
 	OKAY = 0,		//!< No Error - Function executed as expected
 
 	//CAN-Related
@@ -30,7 +33,6 @@ enum ErrorCode
 	BufferFailure = -10,
 	FirwmwareNonFRC = -11,
 
-
 	//General
 	GeneralError = -100,		//!< User Specified General Error
 	GENERAL_ERROR = -100,
@@ -58,24 +60,26 @@ enum ErrorCode
 	GainsAreNotSet = -503,
 	WrongRemoteLimitSwitchSource = -504,
 	DoubleVoltageCompensatingWPI = -505,
+	CANdleAnimSlotOutOfBounds = -506,
 
 	//Higher Level
 	IncompatibleMode = -600,
 	InvalidHandle = -601,		//!< Handle does not match stored map of handles
-	
+
 	//Firmware Versions
 	FeatureRequiresHigherFirm = -700,
     MotorControllerFeatureRequiresHigherFirm = -701,
     TalonFeatureRequiresHigherFirm = MotorControllerFeatureRequiresHigherFirm,
     ConfigFactoryDefaultRequiresHigherFirm = -702,
 	ConfigMotionSCurveRequiresHigherFirm = -703,
 	TalonFXFirmwarePreVBatDetect = -704,
+	CANdleAnimationsRequireHigherFirm = -705,
 
 	//Operating system centric
 	LibraryCouldNotBeLoaded = -800,
 	MissingRoutineInLibrary = -801,
 	ResourceNotAvailable = -802,
-	
+
 	//MIDI and Orchestra centric
 	MusicFileNotFound = -900,
 	MusicFileWrongSize = -901,
@@ -85,7 +89,24 @@ enum ErrorCode
 	MusicFileTooOld = -905,
 	MusicInterrupted = -906,
 	MusicNotSupported = -907,
-
+
+	kInvalidInterface = -1000,
+	kInvalidGuid = -1001,
+	kInvalidClass = -1002,
+	kInvalidProtocol = -1003,
+	kInvalidPath = -1004,
+	kGeneralWinUsbError = -1005,
+	kFailedSetup = -1006,
+	kListenFailed = -1007,
+	kSendFailed = -1008,
+	kReceiveFailed = -1009,
+	kInvalidRespFormat = -1010,
+	kWinUsbInitFailed = -1011,
+	kWinUsbQueryFailed = -1012,
+	kWinUsbGeneralError = -1013,
+	kAccessDenied = -1014,
+	kFirmwareInvalidResponse = -1015,
+
 	//CAN Related
 	PulseWidthSensorNotPresent = +10,	//!< Special Code for "isSensorPresent"
 
@@ -97,11 +118,16 @@ enum ErrorCode
 	FeaturesNotAvailableYet = 104, // feature will be release in an upcoming release
 	ControlModeNotValid = 105, // Current control mode of motor controller not valid for this call
 	ControlModeNotSupportedYet = 106,
-	CascadedPIDNotSupporteYet= 107,
-	AuxiliaryPIDNotSupportedYet= 107,
-	RemoteSensorsNotSupportedYet= 108,
-	MotProfFirmThreshold= 109,
+	CascadedPIDNotSupporteYet = 107,
+	AuxiliaryPIDNotSupportedYet = 107,
+	RemoteSensorsNotSupportedYet = 108,
+	MotProfFirmThreshold = 109,
 	MotProfFirmThreshold2 = 110,
+
+	//Simulation
+	SimDeviceNotFound = 200,
+	SimPhysicsTypeNotSupported = 201,
+	SimDeviceAlreadyExists = 202,
 };
 class ErrorCollection {
 public:
@@ -112,7 +138,7 @@ class ErrorCollection {
         _firstError = FirstOne(_firstError, err);
     }
     void NewError(int err) {
-        _firstError = FirstOne(_firstError, (ErrorCode) err); 
+        _firstError = FirstOne(_firstError, (ErrorCode) err);
     }
 	ErrorCode GetFirstNonZeroError()
 	{

include/ctre/phoenix/PhoenixVersion.h

@@ -0,0 +1 @@
+const int kPhoenixVersion = 0x00051501;

include/ctre/phoenix/cci/CANCoder_CCI.h

@@ -15,7 +15,7 @@
 
 
 extern "C" {
-	CCIEXPORT void* c_CANCoder_Create1(int deviceNumber);
+	CCIEXPORT void* c_CANCoder_Create1(int deviceNumber, const char* canbus);
 	CCIEXPORT void c_CANCoder_DestroyAll();
 	CCIEXPORT ctre::phoenix::ErrorCode c_CANCoder_Destroy(void* handle);
 	CCIEXPORT ctre::phoenix::ErrorCode c_CANCoder_GetDescription(void* handle, char* toFill, int toFillByteSz, size_t* numBytesFilled);

include/ctre/phoenix/cci/CANdle_CCI.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "ctre/phoenix/cci/CCI.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include <set>
+#include <cstddef>
+
+extern "C"{
+    CCIEXPORT void *c_CANdle_Create1(int deviceNumber, const char* canbus);
+    CCIEXPORT void c_CANdle_DestroyAll();
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_Destroy(void *handle);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetDescription(void* handle, char* toFill, int toFillByteSz, size_t* numBytesFilled);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetLastError(void* handle);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetLastTimestamp(void* handle, double* timestamp);
+    CCIEXPORT void c_CANdle_SetLastError(void *handle, int error);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ConfigSetParameter(void *handle, int param, double value, uint8_t subValue, int ordinal, int timeoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ConfigGetParameter(void *handle, int param, double *value, int ordinal, int timeoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ConfigSetCustomParam(void *handle, int newValue, int paramIndex, int timeoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ConfigGetCustomParam(void *handle, int *readValue, int paramIndex, int timoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ConfigFactoryDefault(void *handle, int timeoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetFaults(void *handle, int * param) ;
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetStickyFaults(void *handle, int * param) ;
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ClearStickyFaults(void *handle, int timeoutMs) ;
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetFirmwareVersion(void *handle, int *firmwareVers);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_HasResetOccurred(void *handle, bool * hasReset);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_SetStatusFramePeriod(void *handle, int frame, uint8_t periodMs, int timeoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetStatusFramePeriod(void *handle, int frame, int *periodMs, int timeoutMs);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_SetControlFramePeriod(void *handle, int frame,	int periodMs);
+
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_BlockSet(void *handle, int r, int g, int b, int w, int startIdx, int count);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_SetStandardAnimation(void *handle, int idx, double brightness, double speed, int numLed, int ledOffset, double param4,  double param5, bool reverseDirection, int animSlot);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_SetTwoSizeAnimation(void *handle, int idx, int r, int g, int b, int w, double speed, int numLed, int ledOffset, int direction, int size, int animSlot);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ClearAnimation(void *handle, int animSlot);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_ModulateVBatOutput(void *handle, double percentModulation);
+
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetBusVoltage(void *handle, double *voltage);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_Get5VRailVoltage(void *handle, double *voltage);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetCurrent(void *handle, double *current);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetTemperature(void *handle, double *temperature);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetVBatModulation(void *handle, double *modulation);
+    CCIEXPORT ctre::phoenix::ErrorCode c_CANdle_GetMaxSimultaneousAnimationCount(void *handle, int *maxAnimationCount);
+}

include/ctre/phoenix/cci/CCI.h

@@ -1,3 +1,4 @@
+//Legacy Export Define.  New code should use CTREXPORT from ctre/phoenix/export.h
 #ifndef CCIEXPORT
 
 	#if defined(WIN32) || defined(_WIN32) || defined(_WIN64)

include/ctre/phoenix/cci/Diagnostics_CCI.h

@@ -1,10 +1,11 @@
 #pragma once
 
 #include <stdint.h>
-#include "ctre/phoenix/cci/CCI.h"
+#include "ctre/phoenix/export.h"
 
 extern "C"{
-    CCIEXPORT void c_Phoenix_Diagnostics_Create();
-    CCIEXPORT void c_Phoenix_Diagnostics_Create1(int port);
-    CCIEXPORT void c_Phoenix_Diagnostics_SetSecondsToStart(int secondsToStart);
+    CTREXPORT void c_Phoenix_Diagnostics_Create();
+    CTREXPORT void c_Phoenix_Diagnostics_Create1(int port);
+    CTREXPORT void c_Phoenix_Diagnostics_SetSecondsToStart(int secondsToStart);
+    CTREXPORT void c_Phoenix_Diagnostics_Dispose();
 }

include/ctre/phoenix/cci/MotController_CCI.h

@@ -10,7 +10,7 @@
 
 extern "C"{
 	CCIEXPORT void* c_MotController_Create1(int baseArbId);
-	CCIEXPORT void* c_MotController_Create2(int deviceID, const char * model);
+	CCIEXPORT void* c_MotController_Create2(int deviceID, const char * model, const char* canbus);
     CCIEXPORT void c_MotController_DestroyAll();
     CCIEXPORT ctre::phoenix::ErrorCode c_MotController_Destroy(void *handle);
 	CCIEXPORT ctre::phoenix::ErrorCode c_MotController_GetAppliedControlMode(void *handle, int &controlMode);

include/ctre/phoenix/cci/Orchestra_CCI.h

@@ -14,6 +14,7 @@ extern "C" {
 	CCIEXPORT ctre::phoenix::ErrorCode c_Orchestra_Stop(void* handle);
 	CCIEXPORT ctre::phoenix::ErrorCode c_Orchestra_Pause(void* handle);
 	CCIEXPORT ctre::phoenix::ErrorCode c_Orchestra_IsPlaying(void* handle, bool &playing);
+	CCIEXPORT ctre::phoenix::ErrorCode c_Orchestra_GetCurrentTime(void* handle, uint32_t &currentTime);
 
 	CCIEXPORT ctre::phoenix::ErrorCode c_Orchestra_ClearInstruments(void* handle);
 	CCIEXPORT ctre::phoenix::ErrorCode c_Orchestra_AddInstrument(void* handle, void* instrument);

include/ctre/phoenix/cci/PDP_CCI.h

@@ -0,0 +1,6 @@
+#pragma once
+
+#include "ctre/phoenix/cci/CCI.h"
+#include "ctre/phoenix/ErrorCode.h"
+
+CCIEXPORT ctre::phoenix::ErrorCode c_PDP_GetValues(int deviceID, double *voltage, double currents[], int currentCapacity, int *currentsFilled);