From 28a41e4ac26e0690bc33840cfe84d9b21edfcd09 Mon Sep 17 00:00:00 2001 From: James Kuszmaul Date: Mon, 17 Nov 2014 16:02:41 -0500 Subject: [PATCH] Added support for CAN Talon SRX in C++ and Java. Currently, the JNI bindings are generated by Swig and, unfortunately, the interface available through Java is lower-level than that for C++ (ie, direct access to the ctre code through the JNI bindings, rather than an interface on top of that), but it does work. See eclipse plugins for some short samples. There are a couple of short unit tests as placeholders. Still needs some cleaning up. Change-Id: Iae2f74693ca6b80bf7d5aca0625c66aa6e0b7f85 Added quick samples for C++/Java CAN Talon stuff. Change-Id: I3acb27d6fd5568d88931e0d678c09973d436735d --- .../templates/examples/CANTalon/src/Robot.cpp | 36 + .../resources/templates/examples/examples.xml | 17 + .../org/usfirst/frc/team190/robot/Robot.java | 39 + .../resources/templates/examples/examples.xml | 17 + hal/include/HAL/HAL.hpp | 1 - hal/lib/Athena/CAN.cpp | 232 --- hal/lib/Athena/HAL.cpp | 1 + hal/lib/Athena/ctre/CanTalonSRX.cpp | 609 ++++++++ hal/lib/Athena/ctre/CanTalonSRX.h | 67 + hal/lib/Athena/ctre/CtreCanNode.cpp | 199 +-- hal/lib/Athena/ctre/CtreCanNode.h | 237 ++- wpilibc/wpilibC++Devices/include/CANJaguar.h | 105 +- .../include/CANSpeedController.h | 89 ++ wpilibc/wpilibC++Devices/include/CANTalon.h | 92 ++ wpilibc/wpilibC++Devices/include/WPILib.h | 1 + .../include/ctre/CanTalonSRX.h | 67 + .../include/ctre/CtreCanNode.h | 116 ++ wpilibc/wpilibC++Devices/include/ctre/ctre.h | 60 + wpilibc/wpilibC++Devices/src/CANJaguar.cpp | 48 +- wpilibc/wpilibC++Devices/src/CANTalon.cpp | 516 +++++++ .../src/CANTalonTest.cpp | 21 + .../java/edu/wpi/first/wpilibj/CanTalon.java | 12 + .../edu/wpi/first/wpilibj/CanTalonSRX.java | 238 +++ .../edu/wpi/first/wpilibj/CtreCanNode.java | 46 + .../first/wpilibj/SWIGTYPE_p_CTR_Code.java | 26 + .../wpi/first/wpilibj/SWIGTYPE_p_UINT8.java | 26 + .../wpi/first/wpilibj/SWIGTYPE_p_double.java | 26 + .../edu/wpi/first/wpilibj/SWIGTYPE_p_int.java | 26 + .../wpi/first/wpilibj/hal/CanTalonJNI.java | 67 + .../edu/wpi/first/wpilibj/CANTalonTest.java | 70 + .../edu/wpi/first/wpilibj/SampleTest.java | 7 + .../wpi/first/wpilibj/WpiLibJTestSuite.java | 1 + wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp | 1277 +++++++++++++++++ wpilibj/wpilibJavaJNI/swigTalon/CanTalonSRX.i | 7 + wpilibj/wpilibJavaJNI/swigTalon/README | 5 + .../wpilibJavaJNI/swigTalon/generateJNI.sh | 17 + 36 files changed, 3883 insertions(+), 538 deletions(-) create mode 100644 eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/CANTalon/src/Robot.cpp create mode 100755 eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/CANTalon/src/org/usfirst/frc/team190/robot/Robot.java delete mode 100644 hal/lib/Athena/CAN.cpp create mode 100644 hal/lib/Athena/ctre/CanTalonSRX.cpp create mode 100644 hal/lib/Athena/ctre/CanTalonSRX.h create mode 100644 wpilibc/wpilibC++Devices/include/CANSpeedController.h create mode 100644 wpilibc/wpilibC++Devices/include/CANTalon.h create mode 100644 wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h create mode 100644 wpilibc/wpilibC++Devices/include/ctre/CtreCanNode.h create mode 100644 wpilibc/wpilibC++Devices/include/ctre/ctre.h create mode 100644 wpilibc/wpilibC++Devices/src/CANTalon.cpp create mode 100644 wpilibc/wpilibC++IntegrationTests/src/CANTalonTest.cpp create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalon.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CtreCanNode.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_CTR_Code.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_UINT8.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_double.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_int.java create mode 100644 wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java create mode 100644 wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp create mode 100644 wpilibj/wpilibJavaJNI/swigTalon/CanTalonSRX.i create mode 100644 wpilibj/wpilibJavaJNI/swigTalon/README create mode 100755 wpilibj/wpilibJavaJNI/swigTalon/generateJNI.sh diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/CANTalon/src/Robot.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/CANTalon/src/Robot.cpp new file mode 100644 index 0000000000..fe8c51f036 --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/CANTalon/src/Robot.cpp @@ -0,0 +1,36 @@ +#include "WPILib.h" + +/** + * This is a quick sample program to show how to use the new Talon SRX over CAN. + * As of the time of this writing (11/20/14), the only mode supported on the SRX is the + * straight-up throttle (status info, such as current and temperature should work). + * + */ +class Robot : public SampleRobot { + CANTalon m_motor; + + // update every 0.005 seconds/5 milliseconds. + double kUpdatePeriod = 0.005; + +public: + Robot() + : m_motor(1) // Initialize the Talon as device 1. Use the roboRIO web + // interface to change the device number on the talons. + {} + + /** + * Runs the motor from the output of a Joystick. + */ + void OperatorControl() { + talon.EnableControl(); + while (IsOperatorControl() && IsEnabled()) { + // Takes a number from -1.0 (full reverse) to +1.0 (full forwards). + m_motor.Set(0.5); + + Wait(kUpdatePeriod); // Wait 5ms for the next update. + } + m_motor.Set(0.0); + } +}; + +START_ROBOT_CLASS(Robot); diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml index 84261e94df..4a47e68a18 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml @@ -128,6 +128,23 @@ + + CAN Talon SRX + Quick demo of running the SRX at a given throttle value. + + Robot and Motor + Digital + Actuators + Complete List + + + src + + + + + + Relay Demonstrate controlling a Relay from Joystick buttons. diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/CANTalon/src/org/usfirst/frc/team190/robot/Robot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/CANTalon/src/org/usfirst/frc/team190/robot/Robot.java new file mode 100755 index 0000000000..55ae01955b --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/CANTalon/src/org/usfirst/frc/team190/robot/Robot.java @@ -0,0 +1,39 @@ +package $package; + + +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.hal.CanTalonSRX; +import edu.wpi.first.wpilibj.SampleRobot; +import edu.wpi.first.wpilibj.Timer; + +/** + * This is a short sample program demonstrating how to use the basic throttle + * mode of the new CAN Talon. As of when this was made, the basic throttle mode + * is all that is supported. + */ +public class Robot extends SampleRobot { + + CanTalonSRX motor; + + private final double k_updatePeriod = 0.005; // update every 0.005 seconds/5 milliseconds (200Hz) + + public Robot() { + motor = new CanTalonSRX(1); // Initialize the CanTalonSRX on device 1. + } + + /** + * Runs the motor from a joystick. + */ + public void operatorControl() { + motor.SetModeSelect(0); + while (isOperatorControl() && isEnabled()) { + // Set the motor's output. + // This takes a number from -1 (100% speed in reverse) to +1 (100% speed + // going forward) + motor.Set(0.5); + + Timer.delay(k_updatePeriod); // wait 5ms to the next update + } + motor.Set(0.0); + } +} diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml index 4b6f55e7f5..b0bd47120b 100755 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml @@ -154,6 +154,23 @@ + + CAN Talon SRX + Demonstrate running a Talon SRX with the basic throttle mode. + + Actuators + Complete List + Robot and Motor + + + src/$package-dir + + + + + + CommandBased Robot diff --git a/hal/include/HAL/HAL.hpp b/hal/include/HAL/HAL.hpp index 96c410bf32..39faf8d4ea 100644 --- a/hal/include/HAL/HAL.hpp +++ b/hal/include/HAL/HAL.hpp @@ -14,7 +14,6 @@ #include "Accelerometer.hpp" #include "Analog.hpp" -#include "CAN.hpp" #include "Compressor.hpp" #include "Digital.hpp" #include "Solenoid.hpp" diff --git a/hal/lib/Athena/CAN.cpp b/hal/lib/Athena/CAN.cpp deleted file mode 100644 index c524996b7b..0000000000 --- a/hal/lib/Athena/CAN.cpp +++ /dev/null @@ -1,232 +0,0 @@ -#include "HAL/CAN.hpp" -#include - -struct CANMessage -{ - uint8_t data[8]; -}; - -static std::map outgoingMessages; -static std::map incomingMessages; - -static const uint32_t kFullMessageIDMask = 0x1fffffff; - -/** - * Gets the data from the outgoing hashmap and calls - * CANSessionMux...sendMessage. - */ -void canTxSend(uint32_t arbID, uint8_t length, int32_t period) -{ - CANMessage &message = outgoingMessages[arbID]; - - int32_t status; - - FRC_NetworkCommunication_CANSessionMux_sendMessage( - arbID, message.data, length, period, &status); -} - -/** - * Updates a field in the outgoing hashmap. - * - * This is called every time an single byte field changes in a message.data, - * such as when a setter on a CAN device is called. - */ -void canTxPackInt8(uint32_t arbID, uint8_t offset, uint8_t value) -{ - CANMessage &message = outgoingMessages[arbID]; - - message.data[offset] = value; -} - -/** - * Updates a field in the outgoing hashmap. - * - * This is called every time a short integer field changes in a message.data, - * such as when a setter on a CAN device is called. - */ -void canTxPackInt16(uint32_t arbID, uint8_t offset, uint16_t value) -{ - CANMessage &message = outgoingMessages[arbID]; - - *(uint16_t *)(message.data + offset) = value; -} - -/** - * Updates a field in the outgoing hashmap. - * - * This is called every time a long integer field changes in a message.data, - * such as when a setter on a CAN device is called. - */ -void canTxPackInt32(uint32_t arbID, uint8_t offset, uint32_t value) -{ - CANMessage &message = outgoingMessages[arbID]; - - *(uint32_t *)(message.data + offset) = value; -} - -/** - * Updates a field in the outgoing hashmap. - * - * This is called every time an 8.8 fixed point field changes in a message, - * such as when a setter on a CAN device is called. - */ -void canTxPackFXP16(uint32_t arbID, uint8_t offset, double value) -{ - int16_t raw = value * 255.0; - - canTxPackInt16(arbID, offset, raw); -} - -/** - * Updates a field in the outgoing hashmap. - * - * This is called every time a 16.16 fixed point field changes in a message, - * such as when a setter on a CAN device is called. - */ -void canTxPackFXP32(uint32_t arbID, uint8_t offset, double value) -{ - int32_t raw = value * 65535.0; - - canTxPackInt32(arbID, offset, raw); -} - -/** - * Unpack a field from the outgoing hashmap. - * - * This is called in getters for configuration data. - */ -uint8_t canTxUnpackInt8(uint32_t arbID, uint8_t offset) -{ - CANMessage &message = outgoingMessages[arbID]; - - return message.data[offset]; -} - -/** - * Unpack a field from the outgoing hashmap. - * - * This is called in getters for configuration data. - */ -uint16_t canTxUnpackInt16(uint32_t arbID, uint8_t offset) -{ - CANMessage &message = outgoingMessages[arbID]; - - return *reinterpret_cast(message.data + offset); -} - -/** - * Unpack a field from the outgoing hashmap. - * - * This is called in getters for configuration data. - */ -uint32_t canTxUnpackInt32(uint32_t arbID, uint8_t offset) -{ - CANMessage &message = outgoingMessages[arbID]; - - return *reinterpret_cast(message.data + offset); -} - -/** - * Unpack a field from the outgoing hashmap. - * - * This is called in getters for configuration data. - */ -double canTxUnpackFXP16(uint32_t arbID, uint8_t offset) -{ - int16_t raw = canTxUnpackInt16(arbID, offset); - - return raw / 255.0; -} - -/** - * Unpack a field from the outgoing hashmap. - * - * This is called in getters for configuration data. - */ -double canTxUnpackFXP32(uint32_t arbID, uint8_t offset) -{ - int32_t raw = canTxUnpackInt32(arbID, offset); - - return raw / 65535.0; -} - -/** - * Get data from CANSessionMux (if it's available) and put it in the incoming - * hashmap. - * - * @return true if there's new data. Otherwise, the last received value should - * still be in the hashmap. - */ -bool canRxReceive(uint32_t arbID) -{ - CANMessage &message = incomingMessages[arbID]; - - uint8_t length; - uint32_t timestamp; - int32_t status; - - FRC_NetworkCommunication_CANSessionMux_receiveMessage( - &arbID, kFullMessageIDMask, message.data, &length, ×tamp, &status); - - return status != ERR_CANSessionMux_MessageNotFound; -} - -/** - * Unpack a field from the incoming hashmap. - * - * This is called in getters for status data. - */ -uint8_t canRxUnpackInt8(uint32_t arbID, uint8_t offset) -{ - CANMessage &message = incomingMessages[arbID]; - - return message.data[offset]; -} - -/** - * Unpack a field from the incoming hashmap. - * - * This is called in getters for status data. - */ -uint16_t canRxUnpackInt16(uint32_t arbID, uint8_t offset) -{ - CANMessage &message = incomingMessages[arbID]; - - return *reinterpret_cast(message.data + offset); -} - -/** - * Unpack a field from the incoming hashmap. - * - * This is called in getters for status data. - */ -uint32_t canRxUnpackInt32(uint32_t arbID, uint8_t offset) -{ - CANMessage &message = incomingMessages[arbID]; - - return *reinterpret_cast(message.data + offset); -} - -/** - * Unpack a field from the incoming hashmap. - * - * This is called in getters for status data. - */ -double canRxUnpackFXP16(uint32_t arbID, uint8_t offset) -{ - int16_t raw = canRxUnpackInt16(arbID, offset); - - return raw / 255.0; -} - -/** - * Unpack a field from the incoming hashmap. - * - * This is called in getters for status data. - */ -double canRxUnpackFXP32(uint32_t arbID, uint8_t offset) -{ - int32_t raw = canRxUnpackInt32(arbID, offset); - - return raw / 65535.0; -} diff --git a/hal/lib/Athena/HAL.cpp b/hal/lib/Athena/HAL.cpp index cc4a9d5857..64e1cdba4e 100644 --- a/hal/lib/Athena/HAL.cpp +++ b/hal/lib/Athena/HAL.cpp @@ -8,6 +8,7 @@ #include "NetworkCommunication/FRCComm.h" #include "NetworkCommunication/UsageReporting.h" #include "NetworkCommunication/LoadOut.h" +#include "NetworkCommunication/CANSessionMux.h" #include #include #include diff --git a/hal/lib/Athena/ctre/CanTalonSRX.cpp b/hal/lib/Athena/ctre/CanTalonSRX.cpp new file mode 100644 index 0000000000..715befcf51 --- /dev/null +++ b/hal/lib/Athena/ctre/CanTalonSRX.cpp @@ -0,0 +1,609 @@ +/** + * auto generated using spreadsheet and WpiClassGen.csproj + * @link https://docs.google.com/spreadsheets/d/1OU_ZV7fZLGYUQ-Uhc8sVAmUmWTlT8XBFYK8lfjg_tac/edit#gid=1766046967 + */ +#include "CanTalonSRX.h" +#include "NetworkCommunication/CANSessionMux.h" //CAN Comm +#include // memset +#include // usleep + +#define STATUS_1 0x02041400 +#define STATUS_2 0x02041440 +#define STATUS_3 0x02041480 +#define STATUS_4 0x020414C0 +#define STATUS_5 0x02041500 +#define STATUS_6 0x02041540 +#define STATUS_7 0x02041580 + +#define CONTROL_1 0x02040000 + +#define EXPECTED_RESPONSE_TIMEOUT_MS (200) +#define GET_STATUS1() CtreCanNode::recMsg rx = GetRx(STATUS_1 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) +#define GET_STATUS2() CtreCanNode::recMsg rx = GetRx(STATUS_2 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) +#define GET_STATUS3() CtreCanNode::recMsg rx = GetRx(STATUS_3 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) +#define GET_STATUS4() CtreCanNode::recMsg rx = GetRx(STATUS_4 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) +#define GET_STATUS5() CtreCanNode::recMsg rx = GetRx(STATUS_5 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) +#define GET_STATUS6() CtreCanNode::recMsg rx = GetRx(STATUS_6 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) +#define GET_STATUS7() CtreCanNode::recMsg rx = GetRx(STATUS_7 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) + +/* encoder/decoders */ +/** control */ +typedef struct _TALON_Control_1_General_10ms_t { + unsigned TokenH:8; + unsigned TokenL:8; + unsigned Demand24H:8; + unsigned Demand24M:8; + unsigned Demand24L:8; + unsigned CloseLoopCellSelect:1; + unsigned SelectlFeedbackDevice:4; + unsigned LimitSwitchEn:3; + unsigned RevEncoderPosAndVel:1; + unsigned RevMotDuringCloseLoopEn:1; + unsigned BrakeType:2; + unsigned ModeSelect:4; + unsigned RampThrottle:8; +} TALON_Control_1_General_10ms_t ; +typedef struct _TALON_Control_2_Rates_OneShot_t { + unsigned Status1Ms:8; + unsigned Status2Ms:8; + unsigned Status3Ms:8; + unsigned Status4Ms:8; +} TALON_Control_2_Rates_OneShot_t ; +typedef struct _TALON_Control_3_ClearFlags_OneShot_t { + unsigned ZeroFeedbackSensor:1; + unsigned ClearStickyFaults:1; +} TALON_Control_3_ClearFlags_OneShot_t ; + +/** status */ +typedef struct _TALON_Status_1_General_10ms_t { + unsigned CloseLoopErrH:8; + unsigned CloseLoopErrM:8; + unsigned CloseLoopErrL:8; + unsigned Throttle_h3:3; + unsigned Fault_RevSoftLim:1; + unsigned Fault_ForSoftLim:1; + unsigned TokLocked:1; + unsigned LimitSwitchClosedRev:1; + unsigned LimitSwitchClosedFor:1; + unsigned Throttle_l8:8; + unsigned ModeSelect_h1:1; + unsigned SelectlFeedbackDevice:4; + unsigned LimitSwitchEn:3; + unsigned Fault_HardwareFailure:1; + unsigned Fault_RevLim:1; + unsigned Fault_ForLim:1; + unsigned Fault_UnderVoltage:1; + unsigned Fault_OverTemp:1; + unsigned ModeSelect_b3:3; + unsigned TokenSeed:8; +} TALON_Status_1_General_10ms_t ; +typedef struct _TALON_Status_2_Feedback_20ms_t { + unsigned SensorPositionH:8; + unsigned SensorPositionM:8; + unsigned SensorPositionL:8; + unsigned SensorVelocityH:8; + unsigned SensorVelocityL:8; + unsigned Current_h8:8; + unsigned StckyFault_RevSoftLim:1; + unsigned StckyFault_ForSoftLim:1; + unsigned StckyFault_RevLim:1; + unsigned StckyFault_ForLim:1; + unsigned StckyFault_UnderVoltage:1; + unsigned StckyFault_OverTemp:1; + unsigned Current_l2:2; + unsigned reserved:6; + unsigned CloseLoopCellSelect:1; + unsigned BrakeIsEnabled:1; +} TALON_Status_2_Feedback_20ms_t ; +typedef struct _TALON_Status_3_Enc_100ms_t { + unsigned EncPositionH:8; + unsigned EncPositionM:8; + unsigned EncPositionL:8; + unsigned EncVelH:8; + unsigned EncVelL:8; + unsigned EncIndexRiseEventsH:8; + unsigned EncIndexRiseEventsL:8; + unsigned reserved:5; + unsigned QuadIdxpin:1; + unsigned QuadBpin:1; + unsigned QuadApin:1; +} TALON_Status_3_Enc_100ms_t ; +typedef struct _TALON_Status_4_AinTempVbat_100ms_t { + unsigned AnalogInWithOvH:8; + unsigned AnalogInWithOvM:8; + unsigned AnalogInWithOvL:8; + unsigned AnalogInVelH:8; + unsigned AnalogInVelL:8; + unsigned Temp:8; + unsigned BatteryV:8; + unsigned reserved:8; +} TALON_Status_4_AinTempVbat_100ms_t ; +typedef struct _TALON_Status_5_Startup_OneShot_t { + unsigned ResetCountH:8; + unsigned ResetCountL:8; + unsigned ResetFlagsH:8; + unsigned ResetFlagsL:8; + unsigned FirmVersH:8; + unsigned FirmVersL:8; +} TALON_Status_5_Startup_OneShot_t ; +typedef struct _TALON_Status_6_Eol_t { + unsigned currentAdcUncal_h2:2; + unsigned reserved1:5; + unsigned SpiCsPin_GadgeteerPin6:1; + unsigned currentAdcUncal_l8:8; + unsigned tempAdcUncal_h2:2; + unsigned reserved2:6; + unsigned tempAdcUncal_l8:8; + unsigned vbatAdcUncal_h2:2; + unsigned reserved3:6; + unsigned vbatAdcUncal_l8:8; + unsigned analogAdcUncal_h2:2; + unsigned reserved4:6; + unsigned analogAdcUncal_l8:8; +} TALON_Status_6_Eol_t ; +typedef struct _TALON_Status_7_Debug_200ms_t { + unsigned TokenizationFails_h8:8; + unsigned TokenizationFails_l8:8; + unsigned LastFailedToken_h8:8; + unsigned LastFailedToken_l8:8; + unsigned TokenizationSucceses_h8:8; + unsigned TokenizationSucceses_l8:8; +} TALON_Status_7_Debug_200ms_t ; +typedef struct _TALON_Config_SetGains0_1_t { + unsigned PH:8; + unsigned PM:8; + unsigned PL:8; + unsigned IH:8; + unsigned IM:8; + unsigned IL:8; + unsigned IZoneH:8; + unsigned IZoneL:8; +} TALON_Config_SetGains0_1_t ; +typedef struct _TALON_Config_SetGains0_2_t { + unsigned DH:8; + unsigned DM:8; + unsigned DL:8; + unsigned FH:8; + unsigned FM:8; + unsigned FL:8; + unsigned RampRateH:8; + unsigned RampRateL:8; +} TALON_Config_SetGains0_2_t ; +typedef struct _TALON_Config_SetGains1_1_t { + unsigned PH:8; + unsigned PM:8; + unsigned PL:8; + unsigned IH:8; + unsigned IM:8; + unsigned IL:8; + unsigned IZoneH:8; + unsigned IZoneL:8; +} TALON_Config_SetGains1_1_t ; +typedef struct _TALON_Config_SetGains1_2_t { + unsigned DH:8; + unsigned DM:8; + unsigned DL:8; + unsigned FH:8; + unsigned FM:8; + unsigned FL:8; + unsigned RampRateH:8; + unsigned RampRateL:8; +} TALON_Config_SetGains1_2_t ; +typedef struct _TALON_Config_SetSoftLimits_t { + unsigned LimitFH:8; + unsigned LimitFMH:8; + unsigned LimitFML:8; + unsigned LimitFL:8; + unsigned LimitRH:8; + unsigned LimitRMH:8; + unsigned LimitRML:8; + unsigned LimitRL:8; +} TALON_Config_SetSoftLimits_t ; +typedef struct _TALON_Param_Request_t { + unsigned ParamEnum:8; +} TALON_Param_Request_t ; +typedef struct _TALON_Param_Response_t { + unsigned ParamEnum:8; + unsigned ParamValueH:8; + unsigned ParamValueMH:8; + unsigned ParamValueML:8; + unsigned ParamValueL:8; +} TALON_Param_Response_t ; + + +CanTalonSRX::CanTalonSRX(int deviceNumber): CtreCanNode((UINT8)deviceNumber) +{ + UINT8 device = deviceNumber; + RegisterRx(STATUS_1 | device ); + RegisterRx(STATUS_2 | device ); + RegisterRx(STATUS_3 | device ); + RegisterRx(STATUS_4 | device ); + RegisterRx(STATUS_5 | device ); + RegisterRx(STATUS_6 | device ); + RegisterRx(STATUS_7 | device ); + RegisterTx(CONTROL_1 | device, 10); +} +/* CanTalonSRX D'tor + */ +CanTalonSRX::~CanTalonSRX() +{ +} +void CanTalonSRX::Set(double value) +{ + if(value > 1) + value = 1; + else if(value < -1) + value = -1; + value *= 1023; + SetDemand24(value); /* must be within [-1023,1023] */ +} +/*------------------------ auto generated ----------------------*/ + +CTR_Code CanTalonSRX::GetFault_OverTemp(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_OverTemp; + return rx.err; +} +CTR_Code CanTalonSRX::GetFault_UnderVoltage(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_UnderVoltage; + return rx.err; +} +CTR_Code CanTalonSRX::GetFault_ForLim(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_ForLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetFault_RevLim(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_RevLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetFault_HardwareFailure(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_HardwareFailure; + return rx.err; +} +CTR_Code CanTalonSRX::GetFault_ForSoftLim(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_ForSoftLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetFault_RevSoftLim(int ¶m) +{ + GET_STATUS1(); + param = rx->Fault_RevSoftLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetStckyFault_OverTemp(int ¶m) +{ + GET_STATUS2(); + param = rx->StckyFault_OverTemp; + return rx.err; +} +CTR_Code CanTalonSRX::GetStckyFault_UnderVoltage(int ¶m) +{ + GET_STATUS2(); + param = rx->StckyFault_UnderVoltage; + return rx.err; +} +CTR_Code CanTalonSRX::GetStckyFault_ForLim(int ¶m) +{ + GET_STATUS2(); + param = rx->StckyFault_ForLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetStckyFault_RevLim(int ¶m) +{ + GET_STATUS2(); + param = rx->StckyFault_RevLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetStckyFault_ForSoftLim(int ¶m) +{ + GET_STATUS2(); + param = rx->StckyFault_ForSoftLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetStckyFault_RevSoftLim(int ¶m) +{ + GET_STATUS2(); + param = rx->StckyFault_RevSoftLim; + return rx.err; +} +CTR_Code CanTalonSRX::GetAppliedThrottle11(int ¶m) +{ + GET_STATUS1(); + uint32_t raw = 0; + raw |= rx->Throttle_h3; + raw <<= 8; + raw |= rx->Throttle_l8; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetCloseLoopErr(int ¶m) +{ + GET_STATUS1(); + uint32_t raw = 0; + raw |= rx->CloseLoopErrH; + raw <<= 16; + raw |= rx->CloseLoopErrM; + raw <<= 8; + raw |= rx->CloseLoopErrL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetSelectlFeedbackDevice(int ¶m) +{ + GET_STATUS1(); + param = rx->SelectlFeedbackDevice; + return rx.err; +} +CTR_Code CanTalonSRX::GetModeSelect(int ¶m) +{ + GET_STATUS1(); + uint32_t raw = 0; + raw |= rx->ModeSelect_h1; + raw <<= 3; + raw |= rx->ModeSelect_b3; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetLimitSwitchEn(int ¶m) +{ + GET_STATUS1(); + param = rx->LimitSwitchEn; + return rx.err; +} +CTR_Code CanTalonSRX::GetLimitSwitchClosedFor(int ¶m) +{ + GET_STATUS1(); + param = rx->LimitSwitchClosedFor; + return rx.err; +} +CTR_Code CanTalonSRX::GetLimitSwitchClosedRev(int ¶m) +{ + GET_STATUS1(); + param = rx->LimitSwitchClosedRev; + return rx.err; +} +CTR_Code CanTalonSRX::GetCloseLoopCellSelect(int ¶m) +{ + GET_STATUS2(); + param = rx->CloseLoopCellSelect; + return rx.err; +} +CTR_Code CanTalonSRX::GetSensorPosition(int ¶m) +{ + GET_STATUS2(); + uint32_t raw = 0; + raw |= rx->SensorPositionH; + raw <<= 16; + raw |= rx->SensorPositionM; + raw <<= 8; + raw |= rx->SensorPositionL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetSensorVelocity(int ¶m) +{ + GET_STATUS2(); + uint32_t raw = 0; + raw |= rx->SensorVelocityH; + raw <<= 8; + raw |= rx->SensorVelocityL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetCurrent(double ¶m) +{ + GET_STATUS2(); + uint32_t raw = 0; + raw |= rx->Current_h8; + raw <<= 2; + raw |= rx->Current_l2; + param = (double)raw * 0.125 + 0; + return rx.err; +} +CTR_Code CanTalonSRX::GetBrakeIsEnabled(int ¶m) +{ + GET_STATUS2(); + param = rx->BrakeIsEnabled; + return rx.err; +} +CTR_Code CanTalonSRX::GetEncPosition(int ¶m) +{ + GET_STATUS3(); + uint32_t raw = 0; + raw |= rx->EncPositionH; + raw <<= 16; + raw |= rx->EncPositionM; + raw <<= 8; + raw |= rx->EncPositionL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetEncVel(int ¶m) +{ + GET_STATUS3(); + uint32_t raw = 0; + raw |= rx->EncVelH; + raw <<= 8; + raw |= rx->EncVelL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetEncIndexRiseEvents(int ¶m) +{ + GET_STATUS3(); + uint32_t raw = 0; + raw |= rx->EncIndexRiseEventsH; + raw <<= 8; + raw |= rx->EncIndexRiseEventsL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetQuadApin(int ¶m) +{ + GET_STATUS3(); + param = rx->QuadApin; + return rx.err; +} +CTR_Code CanTalonSRX::GetQuadBpin(int ¶m) +{ + GET_STATUS3(); + param = rx->QuadBpin; + return rx.err; +} +CTR_Code CanTalonSRX::GetQuadIdxpin(int ¶m) +{ + GET_STATUS3(); + param = rx->QuadIdxpin; + return rx.err; +} +CTR_Code CanTalonSRX::GetAnalogInWithOv(int ¶m) +{ + GET_STATUS4(); + uint32_t raw = 0; + raw |= rx->AnalogInWithOvH; + raw <<= 16; + raw |= rx->AnalogInWithOvM; + raw <<= 8; + raw |= rx->AnalogInWithOvL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetAnalogInVel(int ¶m) +{ + GET_STATUS4(); + uint32_t raw = 0; + raw |= rx->AnalogInVelH; + raw <<= 8; + raw |= rx->AnalogInVelL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetTemp(double ¶m) +{ + GET_STATUS4(); + uint32_t raw = rx->Temp; + param = (double)raw * 0.6451612903 + -50; + return rx.err; +} +CTR_Code CanTalonSRX::GetBatteryV(double ¶m) +{ + GET_STATUS4(); + uint32_t raw = rx->BatteryV; + param = (double)raw * 0.05 + 4; + return rx.err; +} +CTR_Code CanTalonSRX::GetResetCount(int ¶m) +{ + GET_STATUS5(); + uint32_t raw = 0; + raw |= rx->ResetCountH; + raw <<= 8; + raw |= rx->ResetCountL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetResetFlags(int ¶m) +{ + GET_STATUS5(); + uint32_t raw = 0; + raw |= rx->ResetFlagsH; + raw <<= 8; + raw |= rx->ResetFlagsL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::GetFirmVers(int ¶m) +{ + GET_STATUS5(); + uint32_t raw = 0; + raw |= rx->FirmVersH; + raw <<= 8; + raw |= rx->FirmVersL; + param = (int)raw; + return rx.err; +} +CTR_Code CanTalonSRX::SetDemand24(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->Demand24H = param>>16; + toFill->Demand24M = param>>8; + toFill->Demand24L = param>>0; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetLimitSwitchEn(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->LimitSwitchEn = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetSelectlFeedbackDevice(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->SelectlFeedbackDevice = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetRevMotDuringCloseLoopEn(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->RevMotDuringCloseLoopEn = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetBrakeType(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->BrakeType = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetModeSelect(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->ModeSelect = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetCloseLoopCellSelect(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->CloseLoopCellSelect = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetRampThrottle(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->RampThrottle = param; + FlushTx(toFill); + return CTR_OKAY; +} +CTR_Code CanTalonSRX::SetRevEncoderPosAndVel(int param) +{ + CtreCanNode::txTask toFill = GetTx(CONTROL_1 | GetDeviceNumber()); + if (toFill.IsEmpty()) return CTR_UnexpectedArbId; + toFill->RevEncoderPosAndVel = param; + FlushTx(toFill); + return CTR_OKAY; +} diff --git a/hal/lib/Athena/ctre/CanTalonSRX.h b/hal/lib/Athena/ctre/CanTalonSRX.h new file mode 100644 index 0000000000..4a9bfe0c58 --- /dev/null +++ b/hal/lib/Athena/ctre/CanTalonSRX.h @@ -0,0 +1,67 @@ +/** + * auto generated using spreadsheet and WpiClassGen.csproj + * @link https://docs.google.com/spreadsheets/d/1OU_ZV7fZLGYUQ-Uhc8sVAmUmWTlT8XBFYK8lfjg_tac/edit#gid=1766046967 + */ +#ifndef CanTalonSRX_H_ +#define CanTalonSRX_H_ +#include "ctre.h" //BIT Defines + Typedefs +#include "CtreCanNode.h" +#include //CAN Comm +class CanTalonSRX : public CtreCanNode +{ +public: + CanTalonSRX(int deviceNumber=0); + ~CanTalonSRX(); + void Set(double value); + + /*------------------------ auto generated ----------------------*/ + CTR_Code GetFault_OverTemp(int ¶m); + CTR_Code GetFault_UnderVoltage(int ¶m); + CTR_Code GetFault_ForLim(int ¶m); + CTR_Code GetFault_RevLim(int ¶m); + CTR_Code GetFault_HardwareFailure(int ¶m); + CTR_Code GetFault_ForSoftLim(int ¶m); + CTR_Code GetFault_RevSoftLim(int ¶m); + CTR_Code GetStckyFault_OverTemp(int ¶m); + CTR_Code GetStckyFault_UnderVoltage(int ¶m); + CTR_Code GetStckyFault_ForLim(int ¶m); + CTR_Code GetStckyFault_RevLim(int ¶m); + CTR_Code GetStckyFault_ForSoftLim(int ¶m); + CTR_Code GetStckyFault_RevSoftLim(int ¶m); + CTR_Code GetAppliedThrottle11(int ¶m); + CTR_Code GetCloseLoopErr(int ¶m); + CTR_Code GetSelectlFeedbackDevice(int ¶m); + CTR_Code GetModeSelect(int ¶m); + CTR_Code GetLimitSwitchEn(int ¶m); + CTR_Code GetLimitSwitchClosedFor(int ¶m); + CTR_Code GetLimitSwitchClosedRev(int ¶m); + CTR_Code GetCloseLoopCellSelect(int ¶m); + CTR_Code GetSensorPosition(int ¶m); + CTR_Code GetSensorVelocity(int ¶m); + CTR_Code GetCurrent(double ¶m); + CTR_Code GetBrakeIsEnabled(int ¶m); + CTR_Code GetEncPosition(int ¶m); + CTR_Code GetEncVel(int ¶m); + CTR_Code GetEncIndexRiseEvents(int ¶m); + CTR_Code GetQuadApin(int ¶m); + CTR_Code GetQuadBpin(int ¶m); + CTR_Code GetQuadIdxpin(int ¶m); + CTR_Code GetAnalogInWithOv(int ¶m); + CTR_Code GetAnalogInVel(int ¶m); + CTR_Code GetTemp(double ¶m); + CTR_Code GetBatteryV(double ¶m); + CTR_Code GetResetCount(int ¶m); + CTR_Code GetResetFlags(int ¶m); + CTR_Code GetFirmVers(int ¶m); + CTR_Code SetDemand24(int param); + CTR_Code SetLimitSwitchEn(int param); + CTR_Code SetSelectlFeedbackDevice(int param); + CTR_Code SetRevMotDuringCloseLoopEn(int param); + CTR_Code SetBrakeType(int param); + CTR_Code SetModeSelect(int param); + CTR_Code SetCloseLoopCellSelect(int param); + CTR_Code SetRampThrottle(int param); + CTR_Code SetRevEncoderPosAndVel(int param); +}; +#endif + diff --git a/hal/lib/Athena/ctre/CtreCanNode.cpp b/hal/lib/Athena/ctre/CtreCanNode.cpp index 30a31a024a..b2e3620284 100644 --- a/hal/lib/Athena/ctre/CtreCanNode.cpp +++ b/hal/lib/Athena/ctre/CtreCanNode.cpp @@ -1,98 +1,101 @@ -#include "CtreCanNode.h" -#include "NetworkCommunication/CANSessionMux.h" -#include // memset -#include // usleep - -static const UINT32 kFullMessageIDMask = 0x1fffffff; - -CtreCanNode::CtreCanNode(UINT8 deviceNumber) -{ - _deviceNumber = deviceNumber; -} -CtreCanNode::~CtreCanNode() -{ -} -void CtreCanNode::RegisterRx(uint32_t arbId) -{ - /* no need to do anything, we just use new API to poll last received message */ -} -void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs) -{ - int32_t status = 0; - - txJob_t job; - job.arbId = arbId; - job.periodMs = periodMs; - _txJobs[arbId] = job; - FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId, - job.toSend, - 8, - job.periodMs, - &status); -} -timespec diff(const timespec & start, const timespec & end) -{ - timespec temp; - if ((end.tv_nsec-start.tv_nsec)<0) { - temp.tv_sec = end.tv_sec-start.tv_sec-1; - temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec; - } else { - temp.tv_sec = end.tv_sec-start.tv_sec; - temp.tv_nsec = end.tv_nsec-start.tv_nsec; - } - return temp; -} -CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs) -{ - CTR_Code retval = CTR_OKAY; - int32_t status = 0; - uint8_t len = 0; - uint32_t timeStamp; - /* cap timeout at 999ms */ - if(timeoutMs > 999) - timeoutMs = 999; - FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status); - if(status == 0){ - /* fresh update */ - rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */ - clock_gettime(2,&r.time); /* fill in time */ - memcpy(r.bytes, dataBytes, 8); /* fill in databytes */ - }else{ - /* did not get the message */ - rxRxEvents_t::iterator i = _rxRxEvents.find(arbId); - if(i == _rxRxEvents.end()){ - /* we've never gotten this mesage */ - retval = CTR_RxTimeout; - /* fill caller's buffer with zeros */ - memset(dataBytes,0,8); - }else{ - /* we've gotten this message before but not recently */ - memcpy(dataBytes,i->second.bytes,8); - /* get the time now */ - struct timespec temp; - clock_gettime(2,&temp); /* get now */ - /* how long has it been? */ - temp = diff(i->second.time,temp); /* temp = now - last */ - if(temp.tv_sec > 0){ - retval = CTR_RxTimeout; - }else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){ - retval = CTR_RxTimeout; - }else { - /* our last update was recent enough */ - } - } - } - - return retval; -} -void CtreCanNode::FlushTx(uint32_t arbId) -{ - int32_t status = 0; - txJobs_t::iterator iter = _txJobs.find(arbId); - if(iter != _txJobs.end()) - FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId, - iter->second.toSend, - 8, - iter->second.periodMs, - &status); -} +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" + +#include "CtreCanNode.h" +#include "NetworkCommunication/CANSessionMux.h" +#include // memset +#include // usleep + +static const UINT32 kFullMessageIDMask = 0x1fffffff; + +CtreCanNode::CtreCanNode(UINT8 deviceNumber) +{ + _deviceNumber = deviceNumber; +} +CtreCanNode::~CtreCanNode() +{ +} +void CtreCanNode::RegisterRx(uint32_t arbId) +{ + /* no need to do anything, we just use new API to poll last received message */ +} +void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs) +{ + int32_t status = 0; + + txJob_t job = {0}; + job.arbId = arbId; + job.periodMs = periodMs; + _txJobs[arbId] = job; + FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId, + job.toSend, + 8, + job.periodMs, + &status); +} +timespec diff(const timespec & start, const timespec & end) +{ + timespec temp; + if ((end.tv_nsec-start.tv_nsec)<0) { + temp.tv_sec = end.tv_sec-start.tv_sec-1; + temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec; + } else { + temp.tv_sec = end.tv_sec-start.tv_sec; + temp.tv_nsec = end.tv_nsec-start.tv_nsec; + } + return temp; +} +CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs) +{ + CTR_Code retval = CTR_OKAY; + int32_t status = 0; + uint8_t len = 0; + uint32_t timeStamp; + /* cap timeout at 999ms */ + if(timeoutMs > 999) + timeoutMs = 999; + FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status); + if(status == 0){ + /* fresh update */ + rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */ + clock_gettime(2,&r.time); /* fill in time */ + memcpy(r.bytes, dataBytes, 8); /* fill in databytes */ + }else{ + /* did not get the message */ + rxRxEvents_t::iterator i = _rxRxEvents.find(arbId); + if(i == _rxRxEvents.end()){ + /* we've never gotten this mesage */ + retval = CTR_RxTimeout; + /* fill caller's buffer with zeros */ + memset(dataBytes,0,8); + }else{ + /* we've gotten this message before but not recently */ + memcpy(dataBytes,i->second.bytes,8); + /* get the time now */ + struct timespec temp; + clock_gettime(2,&temp); /* get now */ + /* how long has it been? */ + temp = diff(i->second.time,temp); /* temp = now - last */ + if(temp.tv_sec > 0){ + retval = CTR_RxTimeout; + }else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){ + retval = CTR_RxTimeout; + }else { + /* our last update was recent enough */ + } + } + } + + return retval; +} +void CtreCanNode::FlushTx(uint32_t arbId) +{ + int32_t status = 0; + txJobs_t::iterator iter = _txJobs.find(arbId); + if(iter != _txJobs.end()) + FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId, + iter->second.toSend, + 8, + iter->second.periodMs, + &status); +} + diff --git a/hal/lib/Athena/ctre/CtreCanNode.h b/hal/lib/Athena/ctre/CtreCanNode.h index 263fcdb2fd..7a2d69054e 100644 --- a/hal/lib/Athena/ctre/CtreCanNode.h +++ b/hal/lib/Athena/ctre/CtreCanNode.h @@ -1,121 +1,116 @@ -#ifndef CtreCanNode_H_ -#define CtreCanNode_H_ -#include "ctre.h" //BIT Defines + Typedefs -#include "NetworkCommunication/CANSessionMux.h" //CAN Comm -#include -#include -#include // memcpy -#include -class CtreCanNode -{ -public: - CtreCanNode(UINT8 deviceNumber); - ~CtreCanNode(); - - UINT8 GetDeviceNumber() - { - return _deviceNumber; - } -protected: - - - template class txTask{ - public: - uint32_t arbId; - T * toSend; - T * operator -> () - { - return toSend; - } - T & operator*() - { - return *toSend; - } - bool IsEmpty() - { - if(toSend == 0) - return true; - return false; - } - }; - template class recMsg{ - public: - uint32_t arbId; - uint8_t bytes[8]; - CTR_Code err; - T * operator -> () - { - return (T *)bytes; - } - T & operator*() - { - return *(T *)bytes; - } - }; - UINT8 _deviceNumber; - void RegisterRx(uint32_t arbId); - void RegisterTx(uint32_t arbId, uint32_t periodMs); - - CTR_Code GetRx(uint32_t arbId,uint8_t * dataBytes,uint32_t timeoutMs); - void FlushTx(uint32_t arbId); - - template txTask GetTx(uint32_t arbId) - { - txTask retval = {0}; - txJobs_t::iterator i = _txJobs.find(arbId); - if(i != _txJobs.end()){ - retval.arbId = i->second.arbId; - retval.toSend = (T*)i->second.toSend; - } - return retval; - } - template void FlushTx(T & par) - { - FlushTx(par.arbId); - } - - template recMsg GetRx(uint32_t arbId, uint32_t timeoutMs) - { - recMsg retval; - retval.err = GetRx(arbId,retval.bytes, timeoutMs); - return retval; - } - -private: - - class txJob_t { - public: - uint32_t arbId; - uint8_t toSend[8]; - uint32_t periodMs; - txJob_t() : arbId(0),periodMs(0) - { - for(unsigned i=0;i txJobs_t; - txJobs_t _txJobs; - - typedef std::map rxRxEvents_t; - rxRxEvents_t _rxRxEvents; -}; -#endif +#ifndef CtreCanNode_H_ +#define CtreCanNode_H_ +#include "ctre.h" //BIT Defines + Typedefs +#include //CAN Comm +#include +#include +#include // memcpy +#include +class CtreCanNode +{ +public: + CtreCanNode(UINT8 deviceNumber); + ~CtreCanNode(); + + UINT8 GetDeviceNumber() + { + return _deviceNumber; + } +protected: + + + template class txTask{ + public: + uint32_t arbId; + T * toSend; + T * operator -> () + { + return toSend; + } + T & operator*() + { + return *toSend; + } + bool IsEmpty() + { + if(toSend == 0) + return true; + return false; + } + }; + template class recMsg{ + public: + uint32_t arbId; + uint8_t bytes[8]; + CTR_Code err; + T * operator -> () + { + return (T *)bytes; + } + T & operator*() + { + return *(T *)bytes; + } + }; + UINT8 _deviceNumber; + void RegisterRx(uint32_t arbId); + void RegisterTx(uint32_t arbId, uint32_t periodMs); + + CTR_Code GetRx(uint32_t arbId,uint8_t * dataBytes,uint32_t timeoutMs); + void FlushTx(uint32_t arbId); + + template txTask GetTx(uint32_t arbId) + { + txTask retval = {0, nullptr}; + txJobs_t::iterator i = _txJobs.find(arbId); + if(i != _txJobs.end()){ + retval.arbId = i->second.arbId; + retval.toSend = (T*)i->second.toSend; + } + return retval; + } + template void FlushTx(T & par) + { + FlushTx(par.arbId); + } + + template recMsg GetRx(uint32_t arbId, uint32_t timeoutMs) + { + recMsg retval; + retval.err = GetRx(arbId,retval.bytes, timeoutMs); + return retval; + } + +private: + + class txJob_t { + public: + uint32_t arbId; + uint8_t toSend[8]; + uint32_t periodMs; + }; + + class rxEvent_t{ + public: + uint8_t bytes[8]; + struct timespec time; + rxEvent_t() + { + bytes[0] = 0; + bytes[1] = 0; + bytes[2] = 0; + bytes[3] = 0; + bytes[4] = 0; + bytes[5] = 0; + bytes[6] = 0; + bytes[7] = 0; + } + }; + + typedef std::map txJobs_t; + txJobs_t _txJobs; + + typedef std::map rxRxEvents_t; + rxRxEvents_t _rxRxEvents; +}; +#endif diff --git a/wpilibc/wpilibC++Devices/include/CANJaguar.h b/wpilibc/wpilibC++Devices/include/CANJaguar.h index 289977d614..6e8e451045 100644 --- a/wpilibc/wpilibC++Devices/include/CANJaguar.h +++ b/wpilibc/wpilibC++Devices/include/CANJaguar.h @@ -10,7 +10,7 @@ #include "Resource.h" #include "MotorSafetyHelper.h" #include "PIDOutput.h" -#include "SpeedController.h" +#include "CANSpeedController.h" #include "HAL/Semaphore.hpp" #include "HAL/HAL.hpp" #include "LiveWindow/LiveWindowSendable.h" @@ -23,7 +23,7 @@ * Luminary Micro Jaguar Speed Control */ class CANJaguar : public MotorSafety, - public SpeedController, + public CANSpeedController, public ErrorBase, public LiveWindowSendable, public ITableListener @@ -41,40 +41,11 @@ public: /** Sets a potentiometer as the position reference only.
Passed as the "tag" when setting the control mode. */ static const struct PotentiometerStruct {} Potentiometer; - typedef enum {kPercentVbus, kCurrent, kSpeed, kPosition, kVoltage} ControlMode; - typedef enum {kCurrentFault = 1, kTemperatureFault = 2, kBusVoltageFault = 4, kGateDriverFault = 8} Faults; - typedef enum {kForwardLimit = 1, kReverseLimit = 2} Limits; - typedef enum { - kNeutralMode_Jumper = 0, /** Use the NeutralMode that is set by the jumper wire on the CAN device */ - kNeutralMode_Brake = 1, /** Stop the motor's rotation by applying a force. */ - kNeutralMode_Coast = 2 /** Do not attempt to stop the motor. Instead allow it to coast to a stop without applying resistance. */ - } NeutralMode; - typedef enum { - /** - * Disables the soft position limits and only uses the limit switches to limit rotation. - * @see CANJaguar#GetForwardLimitOK() - * @see CANJaguar#GetReverseLimitOK() - * - */ - kLimitMode_SwitchInputsOnly = 0, - /** - * Enables the soft position limits on the Jaguar. - * These will be used in addition to the limit switches. This does not disable the behavior - * of the limit switch input. - * @see CANJaguar#ConfigSoftPositionLimits(double, double) - */ - kLimitMode_SoftPositionLimits = 1 - } LimitMode; - explicit CANJaguar(uint8_t deviceNumber); virtual ~CANJaguar(); uint8_t getDeviceNumber() const; - - // SpeedController interface - virtual float Get(); - virtual void Set(float value, uint8_t syncGroup=0); - virtual void Disable(); + uint8_t GetHardwareVersion(); // PIDOutput interface virtual void PIDWrite(float output); @@ -104,39 +75,42 @@ public: void SetVoltageMode(QuadEncoderStruct, uint16_t codesPerRev); void SetVoltageMode(PotentiometerStruct); - // Other Accessors - void SetP(double p); - void SetI(double i); - void SetD(double d); - void SetPID(double p, double i, double d); - double GetP(); - double GetI(); - double GetD(); - float GetBusVoltage(); - float GetOutputVoltage(); - float GetOutputCurrent(); - float GetTemperature(); - double GetPosition(); - double GetSpeed(); - bool GetForwardLimitOK(); - bool GetReverseLimitOK(); - uint16_t GetFaults(); - void SetVoltageRampRate(double rampRate); - virtual uint32_t GetFirmwareVersion(); - uint8_t GetHardwareVersion(); - void ConfigNeutralMode(NeutralMode mode); - void ConfigEncoderCodesPerRev(uint16_t codesPerRev); - void ConfigPotentiometerTurns(uint16_t turns); - void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition); - void DisableSoftPositionLimits(); - void ConfigLimitMode(LimitMode mode); - void ConfigForwardLimit(double forwardLimitPosition); - void ConfigReverseLimit(double reverseLimitPosition); - void ConfigMaxOutputVoltage(double voltage); - void ConfigFaultTime(float faultTime); - ControlMode GetControlMode(); + // CANSpeedController interface + virtual float Get() override; + virtual void Set(float value, uint8_t syncGroup=0) override; + virtual void Disable() override; + virtual void SetP(double p) override; + virtual void SetI(double i) override; + virtual void SetD(double d) override; + virtual void SetPID(double p, double i, double d) override; + virtual double GetP() override; + virtual double GetI() override; + virtual double GetD() override; + virtual float GetBusVoltage() override; + virtual float GetOutputVoltage() override; + virtual float GetOutputCurrent() override; + virtual float GetTemperature() override; + virtual double GetPosition() override; + virtual double GetSpeed() override; + virtual bool GetForwardLimitOK() override; + virtual bool GetReverseLimitOK() override; + virtual uint16_t GetFaults() override; + virtual void SetVoltageRampRate(double rampRate) override; + virtual uint32_t GetFirmwareVersion() override; + virtual void ConfigNeutralMode(NeutralMode mode) override; + virtual void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override; + virtual void ConfigPotentiometerTurns(uint16_t turns) override; + virtual void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) override; + virtual void DisableSoftPositionLimits() override; + virtual void ConfigLimitMode(LimitMode mode) override; + virtual void ConfigForwardLimit(double forwardLimitPosition) override; + virtual void ConfigReverseLimit(double reverseLimitPosition) override; + virtual void ConfigMaxOutputVoltage(double voltage) override; + virtual void ConfigFaultTime(float faultTime) override; + virtual void SetControlMode(ControlMode mode); + virtual ControlMode GetControlMode(); - static void UpdateSyncGroup(uint8_t syncGroup); + static void UpdateSyncGroup(uint8_t syncGroup); void SetExpiration(float timeout); float GetExpiration(); @@ -149,9 +123,6 @@ public: protected: // Control mode helpers - - void ChangeControlMode(ControlMode controlMode); - void SetSpeedReference(uint8_t reference); uint8_t GetSpeedReference(); diff --git a/wpilibc/wpilibC++Devices/include/CANSpeedController.h b/wpilibc/wpilibC++Devices/include/CANSpeedController.h new file mode 100644 index 0000000000..a6a025f519 --- /dev/null +++ b/wpilibc/wpilibC++Devices/include/CANSpeedController.h @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ +/*----------------------------------------------------------------------------*/ +#pragma once + +#include "SpeedController.h" + +/** + * Interface for "smart" CAN-based speed controllers. + * @see CANJaguar + * @see CANTalon + */ +class CANSpeedController : public SpeedController +{ +public: + enum ControlMode { + kPercentVbus=0, + kCurrent=1, + kSpeed=2, + kPosition=3, + kVoltage=4, + kFollower=5 // Not supported in Jaguar. + }; + + enum Faults { + kCurrentFault = 1, + kTemperatureFault = 2, + kBusVoltageFault = 4, + kGateDriverFault = 8 + }; + + enum Limits { + kForwardLimit = 1, + kReverseLimit = 2 + }; + + enum NeutralMode { + /** Use the NeutralMode that is set by the jumper wire on the CAN device */ + kNeutralMode_Jumper = 0, + /** Stop the motor's rotation by applying a force. */ + kNeutralMode_Brake = 1, + /** Do not attempt to stop the motor. Instead allow it to coast to a stop without applying resistance. */ + kNeutralMode_Coast = 2 + }; + + enum LimitMode { + /** Only use switches for limits */ + kLimitMode_SwitchInputsOnly = 0, + /** Use both switches and soft limits */ + kLimitMode_SoftPositionLimits = 1 + }; + + virtual float Get() = 0; + virtual void Set(float value, uint8_t syncGroup=0) = 0; + virtual void Disable() = 0; + virtual void SetP(double p) = 0; + virtual void SetI(double i) = 0; + virtual void SetD(double d) = 0; + virtual void SetPID(double p, double i, double d) = 0; + virtual double GetP() = 0; + virtual double GetI() = 0; + virtual double GetD() = 0; + virtual float GetBusVoltage() = 0; + virtual float GetOutputVoltage() = 0; + virtual float GetOutputCurrent() = 0; + virtual float GetTemperature() = 0; + virtual double GetPosition() = 0; + virtual double GetSpeed() = 0; + virtual bool GetForwardLimitOK() = 0; + virtual bool GetReverseLimitOK() = 0; + virtual uint16_t GetFaults() = 0; + virtual void SetVoltageRampRate(double rampRate) = 0; + virtual uint32_t GetFirmwareVersion() = 0; + virtual void ConfigNeutralMode(NeutralMode mode) = 0; + virtual void ConfigEncoderCodesPerRev(uint16_t codesPerRev) = 0; + virtual void ConfigPotentiometerTurns(uint16_t turns) = 0; + virtual void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) = 0; + virtual void DisableSoftPositionLimits() = 0; + virtual void ConfigLimitMode(LimitMode mode) = 0; + virtual void ConfigForwardLimit(double forwardLimitPosition) = 0; + virtual void ConfigReverseLimit(double reverseLimitPosition) = 0; + virtual void ConfigMaxOutputVoltage(double voltage) = 0; + virtual void ConfigFaultTime(float faultTime) = 0; + // Hold off on interface until we figure out ControlMode enums. +// virtual void SetControlMode(ControlMode mode) = 0; +// virtual ControlMode GetControlMode() = 0; +}; diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h new file mode 100644 index 0000000000..623aa6cdc9 --- /dev/null +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ +/*----------------------------------------------------------------------------*/ +#pragma once + +#include "SafePWM.h" +#include "CANSpeedController.h" +#include "PIDOutput.h" +#include "MotorSafetyHelper.h" + +class CanTalonSRX; + +/** + * CTRE Talon SRX Speed Controller + */ +class CANTalon : public MotorSafety, + public CANSpeedController, + public ErrorBase +{ +public: + explicit CANTalon(int deviceNumber); + virtual ~CANTalon(); + + // PIDController interface + virtual void PIDWrite(float output) override; + + // MotorSafety interface + virtual void SetExpiration(float timeout) override; + virtual float GetExpiration() override; + virtual bool IsAlive() override; + virtual void StopMotor() override; + virtual void SetSafetyEnabled(bool enabled) override; + virtual bool IsSafetyEnabled() override; + virtual void GetDescription(char *desc) override; + + // CANSpeedController interface + virtual float Get() override; + virtual void Set(float value, uint8_t syncGroup=0) override; + virtual void Disable() override; + virtual void EnableControl(); + virtual void SetP(double p) override; + virtual void SetI(double i) override; + virtual void SetD(double d) override; + virtual void SetPID(double p, double i, double d) override; + virtual double GetP() override; + virtual double GetI() override; + virtual double GetD() override; + virtual float GetBusVoltage() override; + virtual float GetOutputVoltage() override; + virtual float GetOutputCurrent() override; + virtual float GetTemperature() override; + virtual double GetPosition() override; + virtual double GetSpeed() override; + virtual bool GetForwardLimitOK() override; + virtual bool GetReverseLimitOK() override; + virtual uint16_t GetFaults() override; + virtual void SetVoltageRampRate(double rampRate) override; + virtual uint32_t GetFirmwareVersion() override; + virtual void ConfigNeutralMode(NeutralMode mode) override; + virtual void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override; + virtual void ConfigPotentiometerTurns(uint16_t turns) override; + virtual void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) override; + virtual void DisableSoftPositionLimits() override; + virtual void ConfigLimitMode(LimitMode mode) override; + virtual void ConfigForwardLimit(double forwardLimitPosition) override; + virtual void ConfigReverseLimit(double reverseLimitPosition) override; + virtual void ConfigMaxOutputVoltage(double voltage) override; + virtual void ConfigFaultTime(float faultTime) override; + virtual void SetControlMode(ControlMode mode); + virtual ControlMode GetControlMode(); + +private: + // Values for various modes as is sent in the CAN packets for the Talon. + enum TalonControlMode { + kThrottle=0, + kFollowerMode=1, + kVoltageMode=2, + kPositionMode=3, + kSpeedMode=4, + kCurrentMode=5, + kDisabled=15 + }; + + int m_deviceNumber; + CanTalonSRX *m_impl; + MotorSafetyHelper *m_safetyHelper; + + bool m_controlEnabled; + ControlMode m_controlMode; +}; diff --git a/wpilibc/wpilibC++Devices/include/WPILib.h b/wpilibc/wpilibC++Devices/include/WPILib.h index 32695a34cb..db1b00e602 100644 --- a/wpilibc/wpilibC++Devices/include/WPILib.h +++ b/wpilibc/wpilibC++Devices/include/WPILib.h @@ -24,6 +24,7 @@ #include "Buttons/NetworkButton.h" #include "CameraServer.h" #include "CANJaguar.h" +#include "CANTalon.h" #include "Commands/Command.h" #include "Commands/CommandGroup.h" #include "Commands/PIDCommand.h" diff --git a/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h b/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h new file mode 100644 index 0000000000..4a9bfe0c58 --- /dev/null +++ b/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h @@ -0,0 +1,67 @@ +/** + * auto generated using spreadsheet and WpiClassGen.csproj + * @link https://docs.google.com/spreadsheets/d/1OU_ZV7fZLGYUQ-Uhc8sVAmUmWTlT8XBFYK8lfjg_tac/edit#gid=1766046967 + */ +#ifndef CanTalonSRX_H_ +#define CanTalonSRX_H_ +#include "ctre.h" //BIT Defines + Typedefs +#include "CtreCanNode.h" +#include //CAN Comm +class CanTalonSRX : public CtreCanNode +{ +public: + CanTalonSRX(int deviceNumber=0); + ~CanTalonSRX(); + void Set(double value); + + /*------------------------ auto generated ----------------------*/ + CTR_Code GetFault_OverTemp(int ¶m); + CTR_Code GetFault_UnderVoltage(int ¶m); + CTR_Code GetFault_ForLim(int ¶m); + CTR_Code GetFault_RevLim(int ¶m); + CTR_Code GetFault_HardwareFailure(int ¶m); + CTR_Code GetFault_ForSoftLim(int ¶m); + CTR_Code GetFault_RevSoftLim(int ¶m); + CTR_Code GetStckyFault_OverTemp(int ¶m); + CTR_Code GetStckyFault_UnderVoltage(int ¶m); + CTR_Code GetStckyFault_ForLim(int ¶m); + CTR_Code GetStckyFault_RevLim(int ¶m); + CTR_Code GetStckyFault_ForSoftLim(int ¶m); + CTR_Code GetStckyFault_RevSoftLim(int ¶m); + CTR_Code GetAppliedThrottle11(int ¶m); + CTR_Code GetCloseLoopErr(int ¶m); + CTR_Code GetSelectlFeedbackDevice(int ¶m); + CTR_Code GetModeSelect(int ¶m); + CTR_Code GetLimitSwitchEn(int ¶m); + CTR_Code GetLimitSwitchClosedFor(int ¶m); + CTR_Code GetLimitSwitchClosedRev(int ¶m); + CTR_Code GetCloseLoopCellSelect(int ¶m); + CTR_Code GetSensorPosition(int ¶m); + CTR_Code GetSensorVelocity(int ¶m); + CTR_Code GetCurrent(double ¶m); + CTR_Code GetBrakeIsEnabled(int ¶m); + CTR_Code GetEncPosition(int ¶m); + CTR_Code GetEncVel(int ¶m); + CTR_Code GetEncIndexRiseEvents(int ¶m); + CTR_Code GetQuadApin(int ¶m); + CTR_Code GetQuadBpin(int ¶m); + CTR_Code GetQuadIdxpin(int ¶m); + CTR_Code GetAnalogInWithOv(int ¶m); + CTR_Code GetAnalogInVel(int ¶m); + CTR_Code GetTemp(double ¶m); + CTR_Code GetBatteryV(double ¶m); + CTR_Code GetResetCount(int ¶m); + CTR_Code GetResetFlags(int ¶m); + CTR_Code GetFirmVers(int ¶m); + CTR_Code SetDemand24(int param); + CTR_Code SetLimitSwitchEn(int param); + CTR_Code SetSelectlFeedbackDevice(int param); + CTR_Code SetRevMotDuringCloseLoopEn(int param); + CTR_Code SetBrakeType(int param); + CTR_Code SetModeSelect(int param); + CTR_Code SetCloseLoopCellSelect(int param); + CTR_Code SetRampThrottle(int param); + CTR_Code SetRevEncoderPosAndVel(int param); +}; +#endif + diff --git a/wpilibc/wpilibC++Devices/include/ctre/CtreCanNode.h b/wpilibc/wpilibC++Devices/include/ctre/CtreCanNode.h new file mode 100644 index 0000000000..59e09398f0 --- /dev/null +++ b/wpilibc/wpilibC++Devices/include/ctre/CtreCanNode.h @@ -0,0 +1,116 @@ +#ifndef CtreCanNode_H_ +#define CtreCanNode_H_ +#include "ctre.h" //BIT Defines + Typedefs +#include //CAN Comm +#include +#include +#include // memcpy +#include +class CtreCanNode +{ +public: + CtreCanNode(UINT8 deviceNumber); + ~CtreCanNode(); + + UINT8 GetDeviceNumber() + { + return _deviceNumber; + } +protected: + + + template class txTask{ + public: + uint32_t arbId; + T * toSend; + T * operator -> () + { + return toSend; + } + T & operator*() + { + return *toSend; + } + bool IsEmpty() + { + if(toSend == 0) + return true; + return false; + } + }; + template class recMsg{ + public: + uint32_t arbId; + uint8_t bytes[8]; + CTR_Code err; + T * operator -> () + { + return (T *)bytes; + } + T & operator*() + { + return *(T *)bytes; + } + }; + UINT8 _deviceNumber; + void RegisterRx(uint32_t arbId); + void RegisterTx(uint32_t arbId, uint32_t periodMs); + + CTR_Code GetRx(uint32_t arbId,uint8_t * dataBytes,uint32_t timeoutMs); + void FlushTx(uint32_t arbId); + + template txTask GetTx(uint32_t arbId) + { + txTask retval = {0}; + txJobs_t::iterator i = _txJobs.find(arbId); + if(i != _txJobs.end()){ + retval.arbId = i->second.arbId; + retval.toSend = (T*)i->second.toSend; + } + return retval; + } + template void FlushTx(T & par) + { + FlushTx(par.arbId); + } + + template recMsg GetRx(uint32_t arbId, uint32_t timeoutMs) + { + recMsg retval; + retval.err = GetRx(arbId,retval.bytes, timeoutMs); + return retval; + } + +private: + + class txJob_t { + public: + uint32_t arbId; + uint8_t toSend[8]; + uint32_t periodMs; + }; + + class rxEvent_t{ + public: + uint8_t bytes[8]; + struct timespec time; + rxEvent_t() + { + bytes[0] = 0; + bytes[1] = 0; + bytes[2] = 0; + bytes[3] = 0; + bytes[4] = 0; + bytes[5] = 0; + bytes[6] = 0; + bytes[7] = 0; + } + }; + + typedef std::map txJobs_t; + txJobs_t _txJobs; + + typedef std::map rxRxEvents_t; + rxRxEvents_t _rxRxEvents; +}; +#endif diff --git a/wpilibc/wpilibC++Devices/include/ctre/ctre.h b/wpilibc/wpilibC++Devices/include/ctre/ctre.h new file mode 100644 index 0000000000..49dc2f6cf3 --- /dev/null +++ b/wpilibc/wpilibC++Devices/include/ctre/ctre.h @@ -0,0 +1,60 @@ +#ifndef GLOBAL_H +#define GLOBAL_H + +//Bit Defines +#define BIT0 0x01 +#define BIT1 0x02 +#define BIT2 0x04 +#define BIT3 0x08 +#define BIT4 0x10 +#define BIT5 0x20 +#define BIT6 0x40 +#define BIT7 0x80 +#define BIT8 0x0100 +#define BIT9 0x0200 +#define BIT10 0x0400 +#define BIT11 0x0800 +#define BIT12 0x1000 +#define BIT13 0x2000 +#define BIT14 0x4000 +#define BIT15 0x8000 + +//Signed +typedef signed char INT8; +typedef signed short INT16; +typedef signed int INT32; +typedef signed long long INT64; + +//Unsigned +typedef unsigned char UINT8; +typedef unsigned short UINT16; +typedef unsigned int UINT32; +typedef unsigned long long UINT64; + +//Other +typedef unsigned char UCHAR; +typedef unsigned short USHORT; +typedef unsigned int UINT; +typedef unsigned long ULONG; + +typedef enum { + CTR_OKAY, //No Error - Function executed as expected + CTR_RxTimeout, /* + * Receive Timeout + * + * No module-specific CAN frames have been received in + * the last 50ms. Function returns the latest received data + * but may be STALE DATA. + */ + CTR_TxTimeout, /* + * Transmission Timeout + * + * No module-specific CAN frames were transmitted in + * the last 50ms. Parameters passed in by the user are loaded + * for next transmission but have not sent. + */ + CTR_InvalidParamValue, + CTR_UnexpectedArbId, +}CTR_Code; + +#endif diff --git a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp index 86f10d936d..a65065a7c7 100644 --- a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp +++ b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp @@ -342,8 +342,9 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) dataSize = packFXP8_8(dataBuffer, outputValue); } break; - default: - return; + default: + wpi_setWPIErrorWithContext(IncompatibleMode, "The Jaguar only supports Current, Voltage, Position, Speed, and Percent (Throttle) modes."); + return; } if (syncGroup != 0) { @@ -1144,6 +1145,7 @@ void CANJaguar::SetP(double p) { case kPercentVbus: case kVoltage: + case kFollower: wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); break; case kSpeed: @@ -1178,6 +1180,7 @@ void CANJaguar::SetI(double i) { case kPercentVbus: case kVoltage: + case kFollower: wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); break; case kSpeed: @@ -1212,6 +1215,7 @@ void CANJaguar::SetD(double d) { case kPercentVbus: case kVoltage: + case kFollower: wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); break; case kSpeed: @@ -1313,6 +1317,9 @@ void CANJaguar::EnableControl(double encoderInitialPosition) case kVoltage: sendMessage(LM_API_VCOMP_T_EN, dataBuffer, dataSize); break; + default: + wpi_setWPIErrorWithContext(IncompatibleMode, "The Jaguar only supports Current, Voltage, Position, Speed, and Percent (Throttle) modes."); + return; } m_controlEnabled = true; @@ -1353,7 +1360,7 @@ void CANJaguar::DisableControl() */ void CANJaguar::SetPercentMode() { - ChangeControlMode(kPercentVbus); + SetControlMode(kPercentVbus); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_NONE); } @@ -1368,7 +1375,7 @@ void CANJaguar::SetPercentMode() */ void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { - ChangeControlMode(kPercentVbus); + SetControlMode(kPercentVbus); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1384,7 +1391,7 @@ void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) */ void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev) { - ChangeControlMode(kPercentVbus); + SetControlMode(kPercentVbus); SetPositionReference(LM_REF_ENCODER); SetSpeedReference(LM_REF_QUAD_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1399,7 +1406,7 @@ void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRe */ void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) { - ChangeControlMode(kPercentVbus); + SetControlMode(kPercentVbus); SetPositionReference(LM_REF_POT); SetSpeedReference(LM_REF_NONE); ConfigPotentiometerTurns(1); @@ -1415,7 +1422,7 @@ void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) */ void CANJaguar::SetCurrentMode(double p, double i, double d) { - ChangeControlMode(kCurrent); + SetControlMode(kCurrent); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_NONE); SetPID(p, i, d); @@ -1433,7 +1440,7 @@ void CANJaguar::SetCurrentMode(double p, double i, double d) */ void CANJaguar::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, double p, double i, double d) { - ChangeControlMode(kCurrent); + SetControlMode(kCurrent); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_NONE); ConfigEncoderCodesPerRev(codesPerRev); @@ -1452,7 +1459,7 @@ void CANJaguar::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, d */ void CANJaguar::SetCurrentMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, double p, double i, double d) { - ChangeControlMode(kCurrent); + SetControlMode(kCurrent); SetPositionReference(LM_REF_ENCODER); SetSpeedReference(LM_REF_QUAD_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1471,7 +1478,7 @@ void CANJaguar::SetCurrentMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRe */ void CANJaguar::SetCurrentMode(CANJaguar::PotentiometerStruct, double p, double i, double d) { - ChangeControlMode(kCurrent); + SetControlMode(kCurrent); SetPositionReference(LM_REF_POT); SetSpeedReference(LM_REF_NONE); ConfigPotentiometerTurns(1); @@ -1491,7 +1498,7 @@ void CANJaguar::SetCurrentMode(CANJaguar::PotentiometerStruct, double p, double */ void CANJaguar::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, double p, double i, double d) { - ChangeControlMode(kSpeed); + SetControlMode(kSpeed); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1510,7 +1517,7 @@ void CANJaguar::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, dou */ void CANJaguar::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, double p, double i, double d) { - ChangeControlMode(kSpeed); + SetControlMode(kSpeed); SetPositionReference(LM_REF_ENCODER); SetSpeedReference(LM_REF_QUAD_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1530,7 +1537,7 @@ void CANJaguar::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, */ void CANJaguar::SetPositionMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, double p, double i, double d) { - ChangeControlMode(kPosition); + SetControlMode(kPosition); SetPositionReference(LM_REF_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); SetPID(p, i, d); @@ -1545,7 +1552,7 @@ void CANJaguar::SetPositionMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerR */ void CANJaguar::SetPositionMode(CANJaguar::PotentiometerStruct, double p, double i, double d) { - ChangeControlMode(kPosition); + SetControlMode(kPosition); SetPositionReference(LM_REF_POT); ConfigPotentiometerTurns(1); SetPID(p, i, d); @@ -1557,7 +1564,7 @@ void CANJaguar::SetPositionMode(CANJaguar::PotentiometerStruct, double p, double */ void CANJaguar::SetVoltageMode() { - ChangeControlMode(kVoltage); + SetControlMode(kVoltage); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_NONE); } @@ -1572,7 +1579,7 @@ void CANJaguar::SetVoltageMode() */ void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { - ChangeControlMode(kVoltage); + SetControlMode(kVoltage); SetPositionReference(LM_REF_NONE); SetSpeedReference(LM_REF_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1588,7 +1595,7 @@ void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) */ void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev) { - ChangeControlMode(kVoltage); + SetControlMode(kVoltage); SetPositionReference(LM_REF_ENCODER); SetSpeedReference(LM_REF_QUAD_ENCODER); ConfigEncoderCodesPerRev(codesPerRev); @@ -1603,7 +1610,7 @@ void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRe */ void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) { - ChangeControlMode(kVoltage); + SetControlMode(kVoltage); SetPositionReference(LM_REF_POT); SetSpeedReference(LM_REF_NONE); ConfigPotentiometerTurns(1); @@ -1618,11 +1625,14 @@ void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) * * @param controlMode The new mode. */ -void CANJaguar::ChangeControlMode(ControlMode controlMode) +void CANJaguar::SetControlMode(ControlMode controlMode) { // Disable the previous mode DisableControl(); + if (controlMode == kFollower) + wpi_setWPIErrorWithContext(IncompatibleMode, "The Jaguar only supports Current, Voltage, Position, Speed, and Percent (Throttle) modes."); + // Update the local mode m_controlMode = controlMode; m_controlModeVerified = false; diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp new file mode 100644 index 0000000000..ae002cc010 --- /dev/null +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -0,0 +1,516 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ +/*----------------------------------------------------------------------------*/ + +#include "CANTalon.h" +#include "WPIErrors.h" +#include "ctre/CanTalonSRX.h" + +/** + * The CANTalon object is currently incomplete. As of Nov 14 2014, we only know + * for sure that sending a throttle and checking basic values (eg current, + * temperature) work. + */ + +/** + * Constructor for the CANTalon device. + * @param deviceNumber The CAN ID of the Talon SRX + */ +CANTalon::CANTalon(int deviceNumber) + : m_deviceNumber(deviceNumber) + , m_impl(new CanTalonSRX(deviceNumber)) + , m_safetyHelper(new MotorSafetyHelper(this)) + , m_controlEnabled(false) +{ + // The control mode may already have been set. + CTR_Code status = m_impl->SetModeSelect((int)m_controlMode); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } +} + +CANTalon::~CANTalon() { + delete m_impl; + delete m_safetyHelper; +} + +/** +* Write out the PID value as seen in the PIDOutput base object. +* +* @deprecated Call Set instead. +* +* @param output Write out the PercentVbus value as was computed by the PIDController +*/ +void CANTalon::PIDWrite(float output) +{ + if (GetControlMode() == kPercentVbus) + { + Set(output); + } + else + { + wpi_setWPIErrorWithContext(IncompatibleMode, "PID only supported in PercentVbus mode"); + } +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +float CANTalon::Get() +{ + return 0.0f; +} + +/** + * Sets the output set-point value. + */ +void CANTalon::Set(float value, uint8_t syncGroup) +{ + if(m_controlEnabled) { + CTR_Code status; + switch(GetControlMode()) { + case kThrottle: + { + m_impl->Set(value); + status = CTR_OKAY; + } + break; + case kFollowerMode: + { + status = m_impl->SetDemand24((int)value); + } + break; + case kVoltageMode: + { + // Voltage is an 8.8 fixed point number. + int volts = int(value * 256); + status = m_impl->SetDemand24(volts); + } + default: + // TODO: Add support for other modes. Need to figure out what format + // SetDemand24 needs. + break; + } + if (status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + } +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::Disable() +{ + // Until Modes other than throttle work, just disable by setting throttle to 0.0. + m_impl->Set(0.0); // TODO when firmware is updated, remove this. + //m_impl->SetModeSelect(kDisabled); // TODO when firmware is updated, uncomment this. + m_controlEnabled = false; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::EnableControl() { + SetControlMode(m_controlMode); + m_controlEnabled = true; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetP(double p) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetI(double i) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetD(double d) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetPID(double p, double i, double d) +{ + SetP(p); + SetI(i); + SetD(d); +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +double CANTalon::GetP() +{ + // TODO + return 0.0; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +double CANTalon::GetI() +{ + // TODO + return 0.0; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +double CANTalon::GetD() +{ + // TODO + return 0.0; +} + +/** + * Returns the voltage coming in from the battery. + * + * @return The input voltage in vols. + */ +float CANTalon::GetBusVoltage() +{ + double voltage; + CTR_Code status = m_impl->GetBatteryV(voltage); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return voltage; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +float CANTalon::GetOutputVoltage() +{ + int throttle11; + CTR_Code status = m_impl->GetAppliedThrottle11(throttle11); + float voltage = GetBusVoltage() * float(throttle11) / 1023.0; + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return voltage; +} + + +/** + * TODO documentation (see CANJaguar.cpp) + */ +float CANTalon::GetOutputCurrent() +{ + double current; + + CTR_Code status = m_impl->GetCurrent(current); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + + return current; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +float CANTalon::GetTemperature() +{ + double temp; + + CTR_Code status = m_impl->GetTemp(temp); + if(temp != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return temp; +} + +/** + * TODO documentation (see CANJaguar.cpp) + * + * @return The position of the sensor currently providing feedback. + */ +double CANTalon::GetPosition() +{ + int postition; + // TODO convert from int to appropriate units (or at least document it). + + CTR_Code status = m_impl->GetSensorPosition(postition); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return (double)postition; +} + +/** + * TODO documentation (see CANJaguar.cpp) + * + * @returns The speed of the sensor currently providing feedback. + */ +double CANTalon::GetSpeed() +{ + int speed; + // TODO convert from int to appropriate units (or at least document it). + + CTR_Code status = m_impl->GetSensorVelocity(speed); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return (double)speed; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +bool CANTalon::GetForwardLimitOK() +{ + // TODO + return false; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +bool CANTalon::GetReverseLimitOK() +{ + // TODO + return false; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +uint16_t CANTalon::GetFaults() +{ + // TODO + return 0; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetVoltageRampRate(double rampRate) +{ + // TODO +} + +/** + * @return The version of the firmware running on the Talon + */ +uint32_t CANTalon::GetFirmwareVersion() +{ + int firmwareVersion; + + CTR_Code status = m_impl->GetFirmVers(firmwareVersion); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + + return firmwareVersion; +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigNeutralMode(NeutralMode mode) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigPotentiometerTurns(uint16_t turns) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::DisableSoftPositionLimits() +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigLimitMode(LimitMode mode) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigForwardLimit(double forwardLimitPosition) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::ConfigReverseLimit(double reverseLimitPosition) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + * Does this exist on the Talon? + */ +void CANTalon::ConfigMaxOutputVoltage(double voltage) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + * Does this exist on the Talon? + */ +void CANTalon::ConfigFaultTime(float faultTime) +{ + // TODO +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) +{ + m_controlMode = mode; + TalonControlMode sendMode; + switch (mode) { + case kPercentVbus: + sendMode = kThrottle; + break; + case kCurrent: + sendMode = kCurrentMode; + break; + case kSpeed: + sendMode = kSpeedMode; + break; + case kPosition: + sendMode = kPositionMode; + break; + case kVoltage: + sendMode = kVoltageMode; + break; + case kFollower: + sendMode = kFollowerMode; + break; + } + CTR_Code status = m_impl->SetModeSelect((int)sendMode); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } +} + +/** + * TODO documentation (see CANJaguar.cpp) + */ +CANSpeedController::ControlMode CANTalon::GetControlMode() +{ + // Take the opportunity to check that the control mode is what we think it is. + int temp; + CTR_Code status = m_impl->GetModeSelect(temp); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + switch ((TalonControlMode)temp) { + case kThrottle: + m_controlMode = kPercentVbus; + break; + case kCurrentMode: + m_controlMode = kCurrent; + break; + case kSpeedMode: + m_controlMode = kSpeed; + break; + case kPositionMode: + m_controlMode = kPosition; + break; + case kVoltageMode: + m_controlMode = kVoltage; + break; + case kFollowerMode: + m_controlMode = kFollower; + break; + case kDisabled: + m_controlEnabled = false; + break; + } + return m_controlMode; +} + +void CANTalon::SetExpiration(float timeout) +{ + m_safetyHelper->SetExpiration(timeout); +} + +float CANTalon::GetExpiration() +{ + return m_safetyHelper->GetExpiration(); +} + +bool CANTalon::IsAlive() +{ + return m_safetyHelper->IsAlive(); +} + +bool CANTalon::IsSafetyEnabled() +{ + return m_safetyHelper->IsSafetyEnabled(); +} + +void CANTalon::SetSafetyEnabled(bool enabled) +{ + m_safetyHelper->SetSafetyEnabled(enabled); +} + +void CANTalon::GetDescription(char *desc) +{ + sprintf(desc, "CANTalon ID %d", m_deviceNumber); +} + +/** + * Common interface for stopping the motor + * Part of the MotorSafety interface + * + * @deprecated Call Disable instead. +*/ +void CANTalon::StopMotor() +{ + Disable(); +} diff --git a/wpilibc/wpilibC++IntegrationTests/src/CANTalonTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/CANTalonTest.cpp new file mode 100644 index 0000000000..57b9054822 --- /dev/null +++ b/wpilibc/wpilibC++IntegrationTests/src/CANTalonTest.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ +/*----------------------------------------------------------------------------*/ + +#include "WPILib.h" +#include "gtest/gtest.h" +#include "TestBench.h" + +TEST(CANTalonTest, QuickTest) { + CANTalon talon(0); + talon.SetControlMode(CANSpeedController::kPercentVbus); + talon.EnableControl(); + talon.Set(1.0); + Wait(0.25); + EXPECT_GT(talon.GetOutputCurrent(), 0.0); + + talon.Set(0.0); + talon.Disable(); +} diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalon.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalon.java new file mode 100644 index 0000000000..e3bf757091 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalon.java @@ -0,0 +1,12 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class CanTalon { +} diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java new file mode 100644 index 0000000000..c1c81293c1 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java @@ -0,0 +1,238 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class CanTalonSRX extends CtreCanNode { + private long swigCPtr; + + protected CanTalonSRX(long cPtr, boolean cMemoryOwn) { + super(CanTalonJNI.CanTalonSRX_SWIGUpcast(cPtr), cMemoryOwn); + swigCPtr = cPtr; + } + + protected static long getCPtr(CanTalonSRX obj) { + return (obj == null) ? 0 : obj.swigCPtr; + } + + protected void finalize() { + delete(); + } + + public synchronized void delete() { + if (swigCPtr != 0) { + if (swigCMemOwn) { + swigCMemOwn = false; + CanTalonJNI.delete_CanTalonSRX(swigCPtr); + } + swigCPtr = 0; + } + super.delete(); + } + + public CanTalonSRX(int deviceNumber) { + this(CanTalonJNI.new_CanTalonSRX__SWIG_0(deviceNumber), true); + } + + public CanTalonSRX() { + this(CanTalonJNI.new_CanTalonSRX__SWIG_1(), true); + } + + public void Set(double value) { + CanTalonJNI.CanTalonSRX_Set(swigCPtr, this, value); + } + + public SWIGTYPE_p_CTR_Code GetFault_OverTemp(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_OverTemp(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFault_UnderVoltage(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_UnderVoltage(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFault_ForLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_ForLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFault_RevLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_RevLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFault_HardwareFailure(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_HardwareFailure(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFault_ForSoftLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_ForSoftLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFault_RevSoftLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_RevSoftLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetStckyFault_OverTemp(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetStckyFault_OverTemp(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetStckyFault_UnderVoltage(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetStckyFault_UnderVoltage(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetStckyFault_ForLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetStckyFault_ForLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetStckyFault_RevLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetStckyFault_RevLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetStckyFault_ForSoftLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetStckyFault_ForSoftLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetStckyFault_RevSoftLim(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetStckyFault_RevSoftLim(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetAppliedThrottle11(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetAppliedThrottle11(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetCloseLoopErr(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetCloseLoopErr(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetSelectlFeedbackDevice(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetSelectlFeedbackDevice(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetModeSelect(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetModeSelect(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetLimitSwitchEn(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetLimitSwitchEn(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetLimitSwitchClosedFor(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetLimitSwitchClosedFor(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetLimitSwitchClosedRev(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetLimitSwitchClosedRev(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetCloseLoopCellSelect(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetCloseLoopCellSelect(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetSensorPosition(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetSensorPosition(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetSensorVelocity(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetSensorVelocity(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetCurrent(SWIGTYPE_p_double param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetCurrent(swigCPtr, this, SWIGTYPE_p_double.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetBrakeIsEnabled(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetBrakeIsEnabled(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetEncPosition(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetEncPosition(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetEncVel(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetEncVel(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetEncIndexRiseEvents(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetEncIndexRiseEvents(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetQuadApin(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetQuadApin(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetQuadBpin(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetQuadBpin(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetQuadIdxpin(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetQuadIdxpin(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetAnalogInWithOv(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetAnalogInWithOv(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetAnalogInVel(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetAnalogInVel(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetTemp(SWIGTYPE_p_double param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetTemp(swigCPtr, this, SWIGTYPE_p_double.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetBatteryV(SWIGTYPE_p_double param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetBatteryV(swigCPtr, this, SWIGTYPE_p_double.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetResetCount(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetResetCount(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetResetFlags(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetResetFlags(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code GetFirmVers(SWIGTYPE_p_int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFirmVers(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); + } + + public SWIGTYPE_p_CTR_Code SetDemand24(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetDemand24(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetLimitSwitchEn(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetLimitSwitchEn(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetSelectlFeedbackDevice(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetSelectlFeedbackDevice(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetRevMotDuringCloseLoopEn(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetRevMotDuringCloseLoopEn(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetBrakeType(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetBrakeType(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetModeSelect(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetModeSelect(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetCloseLoopCellSelect(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetCloseLoopCellSelect(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetRampThrottle(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetRampThrottle(swigCPtr, this, param), true); + } + + public SWIGTYPE_p_CTR_Code SetRevEncoderPosAndVel(int param) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetRevEncoderPosAndVel(swigCPtr, this, param), true); + } + +} diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CtreCanNode.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CtreCanNode.java new file mode 100644 index 0000000000..d1d1540717 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CtreCanNode.java @@ -0,0 +1,46 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class CtreCanNode { + private long swigCPtr; + protected boolean swigCMemOwn; + + protected CtreCanNode(long cPtr, boolean cMemoryOwn) { + swigCMemOwn = cMemoryOwn; + swigCPtr = cPtr; + } + + protected static long getCPtr(CtreCanNode obj) { + return (obj == null) ? 0 : obj.swigCPtr; + } + + protected void finalize() { + delete(); + } + + public synchronized void delete() { + if (swigCPtr != 0) { + if (swigCMemOwn) { + swigCMemOwn = false; + CanTalonJNI.delete_CtreCanNode(swigCPtr); + } + swigCPtr = 0; + } + } + + public CtreCanNode(SWIGTYPE_p_UINT8 deviceNumber) { + this(CanTalonJNI.new_CtreCanNode(SWIGTYPE_p_UINT8.getCPtr(deviceNumber)), true); + } + + public SWIGTYPE_p_UINT8 GetDeviceNumber() { + return new SWIGTYPE_p_UINT8(CanTalonJNI.CtreCanNode_GetDeviceNumber(swigCPtr, this), true); + } + +} diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_CTR_Code.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_CTR_Code.java new file mode 100644 index 0000000000..9246cfec27 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_CTR_Code.java @@ -0,0 +1,26 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class SWIGTYPE_p_CTR_Code { + private long swigCPtr; + + protected SWIGTYPE_p_CTR_Code(long cPtr, boolean futureUse) { + swigCPtr = cPtr; + } + + protected SWIGTYPE_p_CTR_Code() { + swigCPtr = 0; + } + + protected static long getCPtr(SWIGTYPE_p_CTR_Code obj) { + return (obj == null) ? 0 : obj.swigCPtr; + } +} + diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_UINT8.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_UINT8.java new file mode 100644 index 0000000000..6a802b5623 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_UINT8.java @@ -0,0 +1,26 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class SWIGTYPE_p_UINT8 { + private long swigCPtr; + + public SWIGTYPE_p_UINT8(long cPtr, boolean futureUse) { + swigCPtr = cPtr; + } + + protected SWIGTYPE_p_UINT8() { + swigCPtr = 0; + } + + protected static long getCPtr(SWIGTYPE_p_UINT8 obj) { + return (obj == null) ? 0 : obj.swigCPtr; + } +} + diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_double.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_double.java new file mode 100644 index 0000000000..0ba558ef07 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_double.java @@ -0,0 +1,26 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class SWIGTYPE_p_double { + private long swigCPtr; + + protected SWIGTYPE_p_double(long cPtr, boolean futureUse) { + swigCPtr = cPtr; + } + + protected SWIGTYPE_p_double() { + swigCPtr = 0; + } + + protected static long getCPtr(SWIGTYPE_p_double obj) { + return (obj == null) ? 0 : obj.swigCPtr; + } +} + diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_int.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_int.java new file mode 100644 index 0000000000..0e0cbe5ca4 --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SWIGTYPE_p_int.java @@ -0,0 +1,26 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class SWIGTYPE_p_int { + private long swigCPtr; + + protected SWIGTYPE_p_int(long cPtr, boolean futureUse) { + swigCPtr = cPtr; + } + + protected SWIGTYPE_p_int() { + swigCPtr = 0; + } + + protected static long getCPtr(SWIGTYPE_p_int obj) { + return (obj == null) ? 0 : obj.swigCPtr; + } +} + diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java new file mode 100644 index 0000000000..7ceed8708a --- /dev/null +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * Do not make changes to this file unless you know what you are doing--modify + * the SWIG interface file instead. + * ----------------------------------------------------------------------------- */ + +package edu.wpi.first.wpilibj.hal; + +public class CanTalonJNI { + public final static native long new_CtreCanNode(long jarg1); + public final static native void delete_CtreCanNode(long jarg1); + public final static native long CtreCanNode_GetDeviceNumber(long jarg1, CtreCanNode jarg1_); + public final static native long new_CanTalonSRX__SWIG_0(int jarg1); + public final static native long new_CanTalonSRX__SWIG_1(); + public final static native void delete_CanTalonSRX(long jarg1); + public final static native void CanTalonSRX_Set(long jarg1, CanTalonSRX jarg1_, double jarg2); + public final static native long CanTalonSRX_GetFault_OverTemp(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFault_UnderVoltage(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFault_ForLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFault_RevLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFault_HardwareFailure(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFault_ForSoftLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFault_RevSoftLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetStckyFault_OverTemp(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetStckyFault_UnderVoltage(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetStckyFault_ForLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetStckyFault_RevLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetStckyFault_ForSoftLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetStckyFault_RevSoftLim(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetAppliedThrottle11(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetCloseLoopErr(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetSelectlFeedbackDevice(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetModeSelect(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetLimitSwitchEn(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetLimitSwitchClosedFor(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetLimitSwitchClosedRev(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetCloseLoopCellSelect(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetSensorPosition(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetSensorVelocity(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetCurrent(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetBrakeIsEnabled(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetEncPosition(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetEncVel(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetEncIndexRiseEvents(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetQuadApin(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetQuadBpin(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetQuadIdxpin(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetAnalogInWithOv(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetAnalogInVel(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetTemp(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetBatteryV(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetResetCount(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetResetFlags(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_GetFirmVers(long jarg1, CanTalonSRX jarg1_, long jarg2); + public final static native long CanTalonSRX_SetDemand24(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetLimitSwitchEn(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetSelectlFeedbackDevice(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetRevMotDuringCloseLoopEn(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetBrakeType(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetModeSelect(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetCloseLoopCellSelect(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetRampThrottle(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SetRevEncoderPosAndVel(long jarg1, CanTalonSRX jarg1_, int jarg2); + public final static native long CanTalonSRX_SWIGUpcast(long jarg1); +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java new file mode 100644 index 0000000000..2cb7def479 --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ +package edu.wpi.first.wpilibj; + +import static org.junit.Assert.assertTrue; + +import java.util.logging.Logger; + +import org.junit.AfterClass; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import edu.wpi.first.wpilibj.fixtures.SampleFixture; +import edu.wpi.first.wpilibj.test.AbstractComsSetup; + +import edu.wpi.first.wpilibj.hal.CanTalonSRX; + +/** + * Basic test (borrowed straight from SampleTest) for running the CAN TalonSRX. + */ +public class CANTalonTest extends AbstractComsSetup { + private static final Logger logger = Logger.getLogger(SampleTest.class.getName()); + + static SampleFixture fixture = new SampleFixture(); + + protected Logger getClassLogger(){ + return logger; + } + + @BeforeClass + public static void classSetup() { + // Set up the fixture before the test is created + fixture.setup(); + } + + @Before + public void setUp() { + // Reset the fixture elements before every test + fixture.reset(); + } + + @AfterClass + public static void tearDown() { + // Clean up the fixture after the test + fixture.teardown(); + } + + /** + * Briefly run a CAN Talon and assert true. + */ + @Test + public void test() { + // The constructor takes a device ID (settable in roboRIO web interface). + CanTalonSRX talon = new CanTalonSRX(0); + // Make sure that the Talon is in the basic throttle mode. + talon.SetModeSelect(0); + // Set Talon to 50% forwards throttle. + talon.Set(0.5); + Timer.delay(1.5); + // Turn Talon off. + talon.Set(0.0); + assertTrue(true); + } + +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java index 21d34e8acb..70859c26c2 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java @@ -18,6 +18,9 @@ import org.junit.Test; import edu.wpi.first.wpilibj.fixtures.SampleFixture; import edu.wpi.first.wpilibj.test.AbstractComsSetup; +import edu.wpi.first.wpilibj.hal.CanTalonSRX; +import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_UINT8; + /** * Sample test for a sample PID controller. This demonstrates the general * pattern of how to create a test and use testing fixtures and categories. @@ -60,6 +63,10 @@ public class SampleTest extends AbstractComsSetup { */ @Test public void test() { + CanTalonSRX cantalon = new CanTalonSRX(); + cantalon.Set(0.5); + Timer.delay(0.5); + cantalon.Set(0.0); assertTrue(true); } diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index 20c87cf6f8..56ad8dea8f 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; AnalogCrossConnectTest.class, AnalogPotentiometerTest.class, BuiltInAccelerometerTest.class, + CANTalonTest.class, CounterTest.class, DIOCrossConnectTest.class, EncoderTest.class, diff --git a/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp b/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp new file mode 100644 index 0000000000..56d5eb1878 --- /dev/null +++ b/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp @@ -0,0 +1,1277 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 2.0.11 + * + * This file is not intended to be easily readable and contains a number of + * coding conventions designed to improve portability and efficiency. Do not make + * changes to this file unless you know what you are doing--modify the SWIG + * interface file instead. + * ----------------------------------------------------------------------------- */ + +#define SWIGJAVA + + +#ifdef __cplusplus +/* SwigValueWrapper is described in swig.swg */ +template class SwigValueWrapper { + struct SwigMovePointer { + T *ptr; + SwigMovePointer(T *p) : ptr(p) { } + ~SwigMovePointer() { delete ptr; } + SwigMovePointer& operator=(SwigMovePointer& rhs) { T* oldptr = ptr; ptr = 0; delete oldptr; ptr = rhs.ptr; rhs.ptr = 0; return *this; } + } pointer; + SwigValueWrapper& operator=(const SwigValueWrapper& rhs); + SwigValueWrapper(const SwigValueWrapper& rhs); +public: + SwigValueWrapper() : pointer(0) { } + SwigValueWrapper& operator=(const T& t) { SwigMovePointer tmp(new T(t)); pointer = tmp; return *this; } + operator T&() const { return *pointer.ptr; } + T *operator&() { return pointer.ptr; } +}; + +template T SwigValueInit() { + return T(); +} +#endif + +/* ----------------------------------------------------------------------------- + * This section contains generic SWIG labels for method/variable + * declarations/attributes, and other compiler dependent labels. + * ----------------------------------------------------------------------------- */ + +/* template workaround for compilers that cannot correctly implement the C++ standard */ +#ifndef SWIGTEMPLATEDISAMBIGUATOR +# if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x560) +# define SWIGTEMPLATEDISAMBIGUATOR template +# elif defined(__HP_aCC) +/* Needed even with `aCC -AA' when `aCC -V' reports HP ANSI C++ B3910B A.03.55 */ +/* If we find a maximum version that requires this, the test would be __HP_aCC <= 35500 for A.03.55 */ +# define SWIGTEMPLATEDISAMBIGUATOR template +# else +# define SWIGTEMPLATEDISAMBIGUATOR +# endif +#endif + +/* inline attribute */ +#ifndef SWIGINLINE +# if defined(__cplusplus) || (defined(__GNUC__) && !defined(__STRICT_ANSI__)) +# define SWIGINLINE inline +# else +# define SWIGINLINE +# endif +#endif + +/* attribute recognised by some compilers to avoid 'unused' warnings */ +#ifndef SWIGUNUSED +# if defined(__GNUC__) +# if !(defined(__cplusplus)) || (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +# elif defined(__ICC) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +#endif + +#ifndef SWIG_MSC_UNSUPPRESS_4505 +# if defined(_MSC_VER) +# pragma warning(disable : 4505) /* unreferenced local function has been removed */ +# endif +#endif + +#ifndef SWIGUNUSEDPARM +# ifdef __cplusplus +# define SWIGUNUSEDPARM(p) +# else +# define SWIGUNUSEDPARM(p) p SWIGUNUSED +# endif +#endif + +/* internal SWIG method */ +#ifndef SWIGINTERN +# define SWIGINTERN static SWIGUNUSED +#endif + +/* internal inline SWIG method */ +#ifndef SWIGINTERNINLINE +# define SWIGINTERNINLINE SWIGINTERN SWIGINLINE +#endif + +/* exporting methods */ +#if (__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) +# ifndef GCC_HASCLASSVISIBILITY +# define GCC_HASCLASSVISIBILITY +# endif +#endif + +#ifndef SWIGEXPORT +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# if defined(STATIC_LINKED) +# define SWIGEXPORT +# else +# define SWIGEXPORT __declspec(dllexport) +# endif +# else +# if defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) +# define SWIGEXPORT __attribute__ ((visibility("default"))) +# else +# define SWIGEXPORT +# endif +# endif +#endif + +/* calling conventions for Windows */ +#ifndef SWIGSTDCALL +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# define SWIGSTDCALL __stdcall +# else +# define SWIGSTDCALL +# endif +#endif + +/* Deal with Microsoft's attempt at deprecating C standard runtime functions */ +#if !defined(SWIG_NO_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_CRT_SECURE_NO_DEPRECATE) +# define _CRT_SECURE_NO_DEPRECATE +#endif + +/* Deal with Microsoft's attempt at deprecating methods in the standard C++ library */ +#if !defined(SWIG_NO_SCL_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_SCL_SECURE_NO_DEPRECATE) +# define _SCL_SECURE_NO_DEPRECATE +#endif + + + +/* Fix for jlong on some versions of gcc on Windows */ +#if defined(__GNUC__) && !defined(__INTEL_COMPILER) + typedef long long __int64; +#endif + +/* Fix for jlong on 64-bit x86 Solaris */ +#if defined(__x86_64) +# ifdef _LP64 +# undef _LP64 +# endif +#endif + +#include +#include +#include + + +/* Support for throwing Java exceptions */ +typedef enum { + SWIG_JavaOutOfMemoryError = 1, + SWIG_JavaIOException, + SWIG_JavaRuntimeException, + SWIG_JavaIndexOutOfBoundsException, + SWIG_JavaArithmeticException, + SWIG_JavaIllegalArgumentException, + SWIG_JavaNullPointerException, + SWIG_JavaDirectorPureVirtual, + SWIG_JavaUnknownError +} SWIG_JavaExceptionCodes; + +typedef struct { + SWIG_JavaExceptionCodes code; + const char *java_exception; +} SWIG_JavaExceptions_t; + + +static void SWIGUNUSED SWIG_JavaThrowException(JNIEnv *jenv, SWIG_JavaExceptionCodes code, const char *msg) { + jclass excep; + static const SWIG_JavaExceptions_t java_exceptions[] = { + { SWIG_JavaOutOfMemoryError, "java/lang/OutOfMemoryError" }, + { SWIG_JavaIOException, "java/io/IOException" }, + { SWIG_JavaRuntimeException, "java/lang/RuntimeException" }, + { SWIG_JavaIndexOutOfBoundsException, "java/lang/IndexOutOfBoundsException" }, + { SWIG_JavaArithmeticException, "java/lang/ArithmeticException" }, + { SWIG_JavaIllegalArgumentException, "java/lang/IllegalArgumentException" }, + { SWIG_JavaNullPointerException, "java/lang/NullPointerException" }, + { SWIG_JavaDirectorPureVirtual, "java/lang/RuntimeException" }, + { SWIG_JavaUnknownError, "java/lang/UnknownError" }, + { (SWIG_JavaExceptionCodes)0, "java/lang/UnknownError" } + }; + const SWIG_JavaExceptions_t *except_ptr = java_exceptions; + + while (except_ptr->code != code && except_ptr->code) + except_ptr++; + + jenv->ExceptionClear(); + excep = jenv->FindClass(except_ptr->java_exception); + if (excep) + jenv->ThrowNew(excep, msg); +} + + +/* Contract support */ + +#define SWIG_contract_assert(nullreturn, expr, msg) if (!(expr)) {SWIG_JavaThrowException(jenv, SWIG_JavaIllegalArgumentException, msg); return nullreturn; } else + + +#include "ctre/CanTalonSRX.h" + + +#ifdef __cplusplus +extern "C" { +#endif + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CtreCanNode(JNIEnv *jenv, jclass jcls, jlong jarg1) { + jlong jresult = 0 ; + UINT8 arg1 ; + UINT8 *argp1 ; + CtreCanNode *result = 0 ; + + (void)jenv; + (void)jcls; + argp1 = *(UINT8 **)&jarg1; + if (!argp1) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "Attempt to dereference null UINT8"); + return 0; + } + arg1 = *argp1; + result = (CtreCanNode *)new CtreCanNode(arg1); + *(CtreCanNode **)&jresult = result; + return jresult; +} + + +SWIGEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_delete_1CtreCanNode(JNIEnv *jenv, jclass jcls, jlong jarg1) { + CtreCanNode *arg1 = (CtreCanNode *) 0 ; + + (void)jenv; + (void)jcls; + arg1 = *(CtreCanNode **)&jarg1; + delete arg1; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CtreCanNode_1GetDeviceNumber(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_) { + jlong jresult = 0 ; + CtreCanNode *arg1 = (CtreCanNode *) 0 ; + UINT8 result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CtreCanNode **)&jarg1; + result = (arg1)->GetDeviceNumber(); + *(UINT8 **)&jresult = new UINT8((const UINT8 &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_10(JNIEnv *jenv, jclass jcls, jint jarg1) { + jlong jresult = 0 ; + int arg1 ; + CanTalonSRX *result = 0 ; + + (void)jenv; + (void)jcls; + arg1 = (int)jarg1; + result = (CanTalonSRX *)new CanTalonSRX(arg1); + *(CanTalonSRX **)&jresult = result; + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_11(JNIEnv *jenv, jclass jcls) { + jlong jresult = 0 ; + CanTalonSRX *result = 0 ; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX *)new CanTalonSRX(); + *(CanTalonSRX **)&jresult = result; + return jresult; +} + + +SWIGEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_delete_1CanTalonSRX(JNIEnv *jenv, jclass jcls, jlong jarg1) { + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + + (void)jenv; + (void)jcls; + arg1 = *(CanTalonSRX **)&jarg1; + delete arg1; +} + + +SWIGEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1Set(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jdouble jarg2) { + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + double arg2 ; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (double)jarg2; + (arg1)->Set(arg2); +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1OverTemp(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_OverTemp(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1UnderVoltage(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_UnderVoltage(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1ForLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_ForLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1RevLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_RevLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1HardwareFailure(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_HardwareFailure(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1ForSoftLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_ForSoftLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1RevSoftLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFault_RevSoftLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetStckyFault_1OverTemp(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetStckyFault_OverTemp(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetStckyFault_1UnderVoltage(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetStckyFault_UnderVoltage(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetStckyFault_1ForLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetStckyFault_ForLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetStckyFault_1RevLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetStckyFault_RevLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetStckyFault_1ForSoftLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetStckyFault_ForSoftLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetStckyFault_1RevSoftLim(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetStckyFault_RevSoftLim(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetAppliedThrottle11(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetAppliedThrottle11(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetCloseLoopErr(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetCloseLoopErr(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetSelectlFeedbackDevice(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetSelectlFeedbackDevice(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetModeSelect(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetModeSelect(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetLimitSwitchEn(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetLimitSwitchEn(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetLimitSwitchClosedFor(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetLimitSwitchClosedFor(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetLimitSwitchClosedRev(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetLimitSwitchClosedRev(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetCloseLoopCellSelect(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetCloseLoopCellSelect(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetSensorPosition(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetSensorPosition(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetSensorVelocity(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetSensorVelocity(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetCurrent(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + double *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(double **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "double & reference is null"); + return 0; + } + result = (arg1)->GetCurrent(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetBrakeIsEnabled(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetBrakeIsEnabled(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetEncPosition(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetEncPosition(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetEncVel(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetEncVel(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetEncIndexRiseEvents(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetEncIndexRiseEvents(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetQuadApin(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetQuadApin(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetQuadBpin(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetQuadBpin(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetQuadIdxpin(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetQuadIdxpin(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetAnalogInWithOv(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetAnalogInWithOv(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetAnalogInVel(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetAnalogInVel(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetTemp(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + double *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(double **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "double & reference is null"); + return 0; + } + result = (arg1)->GetTemp(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetBatteryV(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + double *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(double **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "double & reference is null"); + return 0; + } + result = (arg1)->GetBatteryV(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetResetCount(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetResetCount(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetResetFlags(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetResetFlags(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFirmVers(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int *arg2 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(int **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int & reference is null"); + return 0; + } + result = (arg1)->GetFirmVers(*arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetDemand24(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetDemand24(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetLimitSwitchEn(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetLimitSwitchEn(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetSelectlFeedbackDevice(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetSelectlFeedbackDevice(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetRevMotDuringCloseLoopEn(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetRevMotDuringCloseLoopEn(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetBrakeType(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetBrakeType(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetModeSelect(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetModeSelect(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetCloseLoopCellSelect(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetCloseLoopCellSelect(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetRampThrottle(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetRampThrottle(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetRevEncoderPosAndVel(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + result = (arg1)->SetRevEncoderPosAndVel(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SWIGUpcast(JNIEnv *jenv, jclass jcls, jlong jarg1) { + jlong baseptr = 0; + (void)jenv; + (void)jcls; + *(CtreCanNode **)&baseptr = *(CanTalonSRX **)&jarg1; + return baseptr; +} + +#ifdef __cplusplus +} +#endif + diff --git a/wpilibj/wpilibJavaJNI/swigTalon/CanTalonSRX.i b/wpilibj/wpilibJavaJNI/swigTalon/CanTalonSRX.i new file mode 100644 index 0000000000..b01ab1d6e4 --- /dev/null +++ b/wpilibj/wpilibJavaJNI/swigTalon/CanTalonSRX.i @@ -0,0 +1,7 @@ +%module CanTalon +%{ +#include "ctre/CanTalonSRX.h" +%} + +%include "CtreCanNode.h" +%include "CanTalonSRX.h" diff --git a/wpilibj/wpilibJavaJNI/swigTalon/README b/wpilibj/wpilibJavaJNI/swigTalon/README new file mode 100644 index 0000000000..59c289a7a5 --- /dev/null +++ b/wpilibj/wpilibJavaJNI/swigTalon/README @@ -0,0 +1,5 @@ +The generateJNI.sh script explains how to use it to generate the JNI bindings +for the CAN Talon stuff using swig. This whole directory is a temporary measure +until I (James Kuszmaul--11/18/2014) or someone else figures out how to +integrate the swig stuff into the build system. For now, all the generated JNI +bindings are checked into git, so that it should work until someone goes and updates ctre/CanTalonSRX.* diff --git a/wpilibj/wpilibJavaJNI/swigTalon/generateJNI.sh b/wpilibj/wpilibJavaJNI/swigTalon/generateJNI.sh new file mode 100755 index 0000000000..d8ac303024 --- /dev/null +++ b/wpilibj/wpilibJavaJNI/swigTalon/generateJNI.sh @@ -0,0 +1,17 @@ +#!/bin/bash +#This script should be able to generate the JNI +# bindings for the CANTalon using swig.At some point, +# it should be integrated into the build system, +# but I[james 18 November 2014] don't know how to do that. +# Assumes running from allwpilib/wpilibj/wpilibJavaJNI/swigTalon +# Get files that we node to generate from. +cp ../../../hal/lib/Athena/ctre/CanTalonSRX.cpp ./ +cp ../../../wpilibc/wpilibC++Devices/include/ctre/* ./ +# Clean up from previous run. +rm *.java +# Run SWIG. +swig -c++ -package edu.wpi.first.wpilibj.hal -java CanTalonSRX.i +# Stick generated files into appropriate places. +cp CanTalonSRX_wrap.cxx ../lib/CanTalonSRXJNI.cpp +mv CanTalonJNI.java ../../wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/ +cp *.java ../../wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/