Squashed 'third_party/allwpilib_2016/' changes from 7f61816..3ce6feb
3ce6feb Updated release number for the new release
e054bbc This adds StopMotor() to the SpeedController interface for C++ and Java. For Java, this is as simple as just adding it, as all motors already have an implementation from MotorSafety that is correctly resolved. For C++, I had to override StopMotor in the classes that descend from SafePWM and explicitly call the SafePWM version. RobotDrive now calls StopMotor on each of its SpeedControllers, instead of calling Disable or setting the motor to 0.0 as it was doing previously.
a15b9dc Merge "More updates to the Gyro test fixing potential null pointer exception"
21b7213 Added Config routine to allow enabling/disabling of limit switch and soft limits. This improves upon the ConfigLimitMode routine, which does not allow certain combinations of enable/disabled limit features. Also keeps parity with LV and Java.
1b45237 Merge "Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically."
1096b15 Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically.
7da21fa More updates to the Gyro test fixing potential null pointer exception
ede5862 Rate-limit duplicate error messages to avoid flooding console.
c22284d Merge "artf4818: Fix CAN Talon JNI references with underscores."
cac3741 Merge "Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now."
097aa8c Fixed the gyro deviation over time test
1d647b3 artf4818: Fix CAN Talon JNI references with underscores.
368cfc7 Merge "Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs."
833e459 Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now.
6c096a3 Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs.
dd7eb0f Fixed robot drive for C++ Simulation
258a622 Merge "Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405"
b1386c6 Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405
a58de40 Merge "Removed publishing of java sim jar"
792d0d3 PDP Classes should support any PDP address
35df955 Merge "Remove maven local as a possible search location"
a0ce9ee Another improvement to HAL-joy getting to ensure it works in future RIO image updates.
0f02c31 Removed publishing of java sim jar
8435ac7 DriverStation::GetJoystickName(): Make work for stick>0.
b4cf4f4 Remove maven local as a possible search location
c3000c3 Merge "Fix HALGetJoystickDescriptor()."
4dec0b4 Merge "Fixed Simulation C++ API"
abc9c27 Fixed Simulation C++ API
b8ae9ec Fix HALGetJoystickDescriptor().
a60f874 Artf4800: Fixes HALGetJoystick*** Segfault
010b584 Merge "fix sim_ds launch script"
4429e16 Merge "Added build dir specification for sim javadoc to not overwrite athena javadoc"
ec9349b Initialized the m_sensors variable to fix artf4798.
9745af8 Added build dir specification for sim javadoc to not overwrite athena javadoc
4da8702 fix sim_ds launch script
05acf79 Fix C++ PIDController SetToleranceBuffer and OnTarget locking.
94a6b05 Merge "Fix onTarget() so that it returns false until there are any values retrieved"
d06053d Fix onTarget() so that it returns false until there are any values retrieved
74927cc Correctly set smart dashboard type for AnalogGyro and ADXRS450_Gyro.
070752f Merge "Fixed sim_ds script library path"
21a8bab Merge "PIDController feed forward term can now be calculated by the end user"
56bd6da Fixed sim_ds script library path
07710f1 Merge "Fixing install script... again"
e1cb61f Use absolute path for NT persistent storage.
09c7482 Fixing install script... again
a3b8bec PIDController feed forward term can now be calculated by the end user
790adb0 artf2612: Update license in source files.
042671c Merge "Removed gz_msgs from wpilibcSim"
c111690 Ultrasonic: replace linked list with std::set.
d71a8ed Removed gz_msgs from wpilibcSim
37259f7 Merge "Replaced linked list in Notifier with std::list"
cd17e7a Merge "Renamed Gyro to AnalogGyro to match athena API"
c5c8a87 Replaced linked list in Notifier with std::list
89405d8 Renamed Gyro to AnalogGyro to match athena API
4a19490 Merge "Adds CANTalon to LiveWindow"
c4a3567 Merge "Fixing the frcsim installer script"
295648f Adds CANTalon to LiveWindow
1b964a2 Merge "Fixes CAN devices in C++ library not showing in the livewindow"
7ba5cee Merge "HAL: Use extern "C" in implementation files."
d17d242 Fixes CAN devices in C++ library not showing in the livewindow
25a771a Added linear digital filters
7349c2c Fixing the frcsim installer script
c82122c Merge "Default bufLength for PIDController in Java should be 1"
58f3f97 Merge "Adds WaitResult to Java waitForInterrupt"
bc8ed12 HAL: Use extern "C" in implementation files.
4cac89e Default bufLength for PIDController in Java should be 1
64fcdcc Keep track of FPGA time rollovers with 64-bit time.
d30b283 Merge "Change C++ Notifier to allow std::function callback."
6ee3052 Merge "Rewrite C++ Notifier to use HAL multi-notifier support."
f5d09e2 Merge "Rewrite Java Notifier and update Interrupt JNI."
d0274aa Merge "Readded styleguide accidentally removed in the reorg"
68311ad Merge "Artf4179: Allow alternate I2C addresses for ADXL345_I2C"
dee12d4 Readded styleguide accidentally removed in the reorg
fa100df Fixed some typos in the comments of MotorEncoderFixture.java, a method name in CANMotorEncoderFixture.java, and the README files
3397b5c Adds WaitResult to Java waitForInterrupt
5f0dffd Artf4177: Use read byte count for ReadString
8564f33 Artf4179: Allow alternate I2C addresses for ADXL345_I2C
e52b52d Change C++ Notifier to allow std::function callback.
40b29e7 Rewrite C++ Notifier to use HAL multi-notifier support.
d126f45 Rewrite Java Notifier and update Interrupt JNI.
557805a Merge "finishing up FRCSim installer"
911b64b finishing up FRCSim installer
e24fe6f Merge "Artf4776 Fixes First DIO PWM usage errors"
f8f9284 Merge "Artf4774 Fixes HAL getHALErrorMessage missing error"
84428d5 Merge "Prevent double free in DigitalGlitchFilter"
a00a5ff Merge "Set correct error message"
9aeee98 Prevent double free in DigitalGlitchFilter
5d2186c working on install process for FRCSim 2016
7fdb616 Merge "This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places."
f67ebca Improved READMEs
8a7c019 Artf4776 Fixes First DIO PWM usage errors
1cd2f9a Added libnipalu to make vision programs link properly
375b92a This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places.
8d0a990 Set correct error message
b65401f Artf4774 Fixes HAL getHALErrorMessage missing error
5f918be Condition java sim build on -PmakeSim flag
53bd180 Merge "Add SPARK and SD540 motor controllers"
66cbe69 Fixed double free of DriverStation.
431f345 Repaired simulation build on linux
611593c Add Cmake wrappers and unzip desktop ntcore builds
51a18cd Add SPARK and SD540 motor controllers
c05e883 Merge changes I55ce71c6,I803680c1
2b80029 Rewrite CANTalon JNI layer.
ef4c45b Last feature addition for CANTalon java/C++ user-facing API.
Change-Id: Ia3a124978a426991890b6f8abbe07d34d75ba38d
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 3ce6feb8acdeca46e93a55280fb6ace3a4d5bcd6
diff --git a/hal/include/HAL/CanTalonSRX.h b/hal/include/HAL/CanTalonSRX.h
index 4cbff71..87e2bf0 100644
--- a/hal/include/HAL/CanTalonSRX.h
+++ b/hal/include/HAL/CanTalonSRX.h
@@ -1,421 +1,829 @@
-/**
- * @brief CAN TALON SRX driver.
- *
- * The TALON SRX is designed to instrument all runtime signals periodically. The default periods are chosen to support 16 TALONs
- * with 10ms update rate for control (throttle or setpoint). However these can be overridden with SetStatusFrameRate. @see SetStatusFrameRate
- * The getters for these unsolicited signals are auto generated at the bottom of this module.
- *
- * Likewise most control signals are sent periodically using the fire-and-forget CAN API.
- * The setters for these unsolicited signals are auto generated at the bottom of this module.
- *
- * Signals that are not available in an unsolicited fashion are the Close Loop gains.
- * For teams that have a single profile for their TALON close loop they can use either the webpage to configure their TALONs once
- * or set the PIDF,Izone,CloseLoopRampRate,etc... once in the robot application. These parameters are saved to flash so once they are
- * loaded in the TALON, they will persist through power cycles and mode changes.
- *
- * For teams that have one or two profiles to switch between, they can use the same strategy since there are two slots to choose from
- * and the ProfileSlotSelect is periodically sent in the 10 ms control frame.
- *
- * For teams that require changing gains frequently, they can use the soliciting API to get and set those parameters. Most likely
- * they will only need to set them in a periodic fashion as a function of what motion the application is attempting.
- * If this API is used, be mindful of the CAN utilization reported in the driver station.
- *
- * Encoder position is measured in encoder edges. Every edge is counted (similar to roboRIO 4X mode).
- * Analog position is 10 bits, meaning 1024 ticks per rotation (0V => 3.3V).
- * Use SetFeedbackDeviceSelect to select which sensor type you need. Once you do that you can use GetSensorPosition()
- * and GetSensorVelocity(). These signals are updated on CANBus every 20ms (by default).
- * If a relative sensor is selected, you can zero (or change the current value) using SetSensorPosition.
- *
- * Analog Input and quadrature position (and velocity) are also explicitly reported in GetEncPosition, GetEncVel, GetAnalogInWithOv, GetAnalogInVel.
- * These signals are available all the time, regardless of what sensor is selected at a rate of 100ms. This allows easy instrumentation
- * for "in the pits" checking of all sensors regardless of modeselect. The 100ms rate is overridable for teams who want to acquire sensor
- * data for processing, not just instrumentation. Or just select the sensor using SetFeedbackDeviceSelect to get it at 20ms.
- *
- * Velocity is in position ticks / 100ms.
- *
- * All output units are in respect to duty cycle (throttle) which is -1023(full reverse) to +1023 (full forward).
- * This includes demand (which specifies duty cycle when in duty cycle mode) and rampRamp, which is in throttle units per 10ms (if nonzero).
- *
- * Pos and velocity close loops are calc'd as
- * err = target - posOrVel.
- * iErr += err;
- * if( (IZone!=0) and abs(err) > IZone)
- * ClearIaccum()
- * output = P X err + I X iErr + D X dErr + F X target
- * dErr = err - lastErr
- * P, I,and D gains are always positive. F can be negative.
- * Motor direction can be reversed using SetRevMotDuringCloseLoopEn if sensor and motor are out of phase.
- * Similarly feedback sensor can also be reversed (multiplied by -1) if you prefer the sensor to be inverted.
- *
- * P gain is specified in throttle per error tick. For example, a value of 102 is ~9.9% (which is 102/1023) throttle per 1
- * ADC unit(10bit) or 1 quadrature encoder edge depending on selected sensor.
- *
- * I gain is specified in throttle per integrated error. For example, a value of 10 equates to ~0.99% (which is 10/1023)
- * for each accumulated ADC unit(10bit) or 1 quadrature encoder edge depending on selected sensor.
- * Close loop and integral accumulator runs every 1ms.
- *
- * D gain is specified in throttle per derivative error. For example a value of 102 equates to ~9.9% (which is 102/1023)
- * per change of 1 unit (ADC or encoder) per ms.
- *
- * I Zone is specified in the same units as sensor position (ADC units or quadrature edges). If pos/vel error is outside of
- * this value, the integrated error will auto-clear...
- * if( (IZone!=0) and abs(err) > IZone)
- * ClearIaccum()
- * ...this is very useful in preventing integral windup and is highly recommended if using full PID to keep stability low.
- *
- * CloseLoopRampRate is in throttle units per 1ms. Set to zero to disable ramping.
- * Works the same as RampThrottle but only is in effect when a close loop mode and profile slot is selected.
- *
- * 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/ctre.h" //BIT Defines + Typedefs
-#include "ctre/CtreCanNode.h"
-#include <FRC_NetworkCommunication/CANSessionMux.h> //CAN Comm
-#include <map>
-#include <atomic>
-class CanTalonSRX : public CtreCanNode
-{
-private:
- // Use this for determining whether the default move constructor has been
- // called; this prevents us from calling the destructor twice.
- struct HasBeenMoved {
- HasBeenMoved(HasBeenMoved&& other) {
- other.moved = true;
- moved = false;
- }
- HasBeenMoved() = default;
- std::atomic<bool> moved{false};
- operator bool() const { return moved; }
- } m_hasBeenMoved;
-
- //---------------------- Vars for opening a CAN stream if caller needs signals that require soliciting */
- uint32_t _can_h; //!< Session handle for catching response params.
- int32_t _can_stat; //!< Session handle status.
- struct tCANStreamMessage _msgBuff[20];
- static int const kMsgCapacity = 20;
- typedef std::map<uint32_t, uint32_t> sigs_t;
- sigs_t _sigs; //!< Catches signal updates that are solicited. Expect this to be very few.
- void OpenSessionIfNeedBe();
- void ProcessStreamMessages();
- /**
- * Send a one shot frame to set an arbitrary signal.
- * Most signals are in the control frame so avoid using this API unless you have to.
- * Use this api for...
- * -A motor controller profile signal eProfileParam_XXXs. These are backed up in flash. If you are gain-scheduling then call this periodically.
- * -Default brake and limit switch signals... eOnBoot_XXXs. Avoid doing this, use the override signals in the control frame.
- * Talon will automatically send a PARAM_RESPONSE after the set, so GetParamResponse will catch the latest value after a couple ms.
- */
- CTR_Code SetParamRaw(uint32_t paramEnum, int32_t rawBits);
- /**
- * Checks cached CAN frames and updating solicited signals.
- */
- CTR_Code GetParamResponseRaw(uint32_t paramEnum, int32_t & rawBits);
-public:
- static const int kDefaultControlPeriodMs = 10; //!< default control update rate is 10ms.
- CanTalonSRX(int deviceNumber = 0,int controlPeriodMs = kDefaultControlPeriodMs);
- ~CanTalonSRX();
- void Set(double value);
- /* mode select enumerations */
- static const int kMode_DutyCycle = 0; //!< Demand is 11bit signed duty cycle [-1023,1023].
- static const int kMode_PositionCloseLoop = 1; //!< Position PIDF.
- static const int kMode_VelocityCloseLoop = 2; //!< Velocity PIDF.
- static const int kMode_CurrentCloseLoop = 3; //!< Current close loop - not done.
- static const int kMode_VoltCompen = 4; //!< Voltage Compensation Mode - not done. Demand is fixed pt target 8.8 volts.
- static const int kMode_SlaveFollower = 5; //!< Demand is the 6 bit Device ID of the 'master' TALON SRX.
- static const int kMode_NoDrive = 15; //!< Zero the output (honors brake/coast) regardless of demand. Might be useful if we need to change modes but can't atomically change all the signals we want in between.
- /* limit switch enumerations */
- static const int kLimitSwitchOverride_UseDefaultsFromFlash = 1;
- static const int kLimitSwitchOverride_DisableFwd_DisableRev = 4;
- static const int kLimitSwitchOverride_DisableFwd_EnableRev = 5;
- static const int kLimitSwitchOverride_EnableFwd_DisableRev = 6;
- static const int kLimitSwitchOverride_EnableFwd_EnableRev = 7;
- /* brake override enumerations */
- static const int kBrakeOverride_UseDefaultsFromFlash = 0;
- static const int kBrakeOverride_OverrideCoast = 1;
- static const int kBrakeOverride_OverrideBrake = 2;
- /* feedback device enumerations */
- static const int kFeedbackDev_DigitalQuadEnc=0;
- static const int kFeedbackDev_AnalogPot=2;
- static const int kFeedbackDev_AnalogEncoder=3;
- static const int kFeedbackDev_CountEveryRisingEdge=4;
- static const int kFeedbackDev_CountEveryFallingEdge=5;
- static const int kFeedbackDev_PosIsPulseWidth=8;
- /* ProfileSlotSelect enumerations*/
- static const int kProfileSlotSelect_Slot0 = 0;
- static const int kProfileSlotSelect_Slot1 = 1;
- /* status frame rate types */
- static const int kStatusFrame_General = 0;
- static const int kStatusFrame_Feedback = 1;
- static const int kStatusFrame_Encoder = 2;
- static const int kStatusFrame_AnalogTempVbat = 3;
- static const int kStatusFrame_PulseWidthMeas = 4;
- /**
- * Signal enumeration for generic signal access.
- * Although every signal is enumerated, only use this for traffic that must be solicited.
- * Use the auto generated getters/setters at bottom of this header as much as possible.
- */
- typedef enum _param_t{
- eProfileParamSlot0_P=1,
- eProfileParamSlot0_I=2,
- eProfileParamSlot0_D=3,
- eProfileParamSlot0_F=4,
- eProfileParamSlot0_IZone=5,
- eProfileParamSlot0_CloseLoopRampRate=6,
- eProfileParamSlot1_P=11,
- eProfileParamSlot1_I=12,
- eProfileParamSlot1_D=13,
- eProfileParamSlot1_F=14,
- eProfileParamSlot1_IZone=15,
- eProfileParamSlot1_CloseLoopRampRate=16,
- eProfileParamSoftLimitForThreshold=21,
- eProfileParamSoftLimitRevThreshold=22,
- eProfileParamSoftLimitForEnable=23,
- eProfileParamSoftLimitRevEnable=24,
- eOnBoot_BrakeMode=31,
- eOnBoot_LimitSwitch_Forward_NormallyClosed=32,
- eOnBoot_LimitSwitch_Reverse_NormallyClosed=33,
- eOnBoot_LimitSwitch_Forward_Disable=34,
- eOnBoot_LimitSwitch_Reverse_Disable=35,
- eFault_OverTemp=41,
- eFault_UnderVoltage=42,
- eFault_ForLim=43,
- eFault_RevLim=44,
- eFault_HardwareFailure=45,
- eFault_ForSoftLim=46,
- eFault_RevSoftLim=47,
- eStckyFault_OverTemp=48,
- eStckyFault_UnderVoltage=49,
- eStckyFault_ForLim=50,
- eStckyFault_RevLim=51,
- eStckyFault_ForSoftLim=52,
- eStckyFault_RevSoftLim=53,
- eAppliedThrottle=61,
- eCloseLoopErr=62,
- eFeedbackDeviceSelect=63,
- eRevMotDuringCloseLoopEn=64,
- eModeSelect=65,
- eProfileSlotSelect=66,
- eRampThrottle=67,
- eRevFeedbackSensor=68,
- eLimitSwitchEn=69,
- eLimitSwitchClosedFor=70,
- eLimitSwitchClosedRev=71,
- eSensorPosition=73,
- eSensorVelocity=74,
- eCurrent=75,
- eBrakeIsEnabled=76,
- eEncPosition=77,
- eEncVel=78,
- eEncIndexRiseEvents=79,
- eQuadApin=80,
- eQuadBpin=81,
- eQuadIdxpin=82,
- eAnalogInWithOv=83,
- eAnalogInVel=84,
- eTemp=85,
- eBatteryV=86,
- eResetCount=87,
- eResetFlags=88,
- eFirmVers=89,
- eSettingsChanged=90,
- eQuadFilterEn=91,
- ePidIaccum=93,
- eStatus1FrameRate=94, // TALON_Status_1_General_10ms_t
- eStatus2FrameRate=95, // TALON_Status_2_Feedback_20ms_t
- eStatus3FrameRate=96, // TALON_Status_3_Enc_100ms_t
- eStatus4FrameRate=97, // TALON_Status_4_AinTempVbat_100ms_t
- eStatus6FrameRate=98, // TALON_Status_6_Eol_t
- eStatus7FrameRate=99, // TALON_Status_7_Debug_200ms_t
- eClearPositionOnIdx=100,
- //reserved,
- //reserved,
- //reserved,
- ePeakPosOutput=104,
- eNominalPosOutput=105,
- ePeakNegOutput=106,
- eNominalNegOutput=107,
- eQuadIdxPolarity=108,
- eStatus8FrameRate=109, // TALON_Status_8_PulseWid_100ms_t
- eAllowPosOverflow=110,
- eProfileParamSlot0_AllowableClosedLoopErr=111,
- eNumberPotTurns=112,
- eNumberEncoderCPR=113,
- ePwdPosition=114,
- eAinPosition=115,
- eProfileParamVcompRate=116,
- eProfileParamSlot1_AllowableClosedLoopErr=117,
- }param_t;
- /*---------------------setters and getters that use the solicated param request/response-------------*//**
- * Send a one shot frame to set an arbitrary signal.
- * Most signals are in the control frame so avoid using this API unless you have to.
- * Use this api for...
- * -A motor controller profile signal eProfileParam_XXXs. These are backed up in flash. If you are gain-scheduling then call this periodically.
- * -Default brake and limit switch signals... eOnBoot_XXXs. Avoid doing this, use the override signals in the control frame.
- * Talon will automatically send a PARAM_RESPONSE after the set, so GetParamResponse will catch the latest value after a couple ms.
- */
- CTR_Code SetParam(param_t paramEnum, double value);
- /**
- * Asks TALON to immedietely respond with signal value. This API is only used for signals that are not sent periodically.
- * This can be useful for reading params that rarely change like Limit Switch settings and PIDF values.
- * @param param to request.
- */
- CTR_Code RequestParam(param_t paramEnum);
- CTR_Code GetParamResponse(param_t paramEnum, double & value);
- CTR_Code GetParamResponseInt32(param_t paramEnum, int & value);
- /*----- getters and setters that use param request/response. These signals are backed up in flash and will survive a power cycle. ---------*/
- /*----- If your application requires changing these values consider using both slots and switch between slot0 <=> slot1. ------------------*/
- /*----- If your application requires changing these signals frequently then it makes sense to leverage this API. --------------------------*/
- /*----- Getters don't block, so it may require several calls to get the latest value. --------------------------*/
- CTR_Code SetPgain(unsigned slotIdx,double gain);
- CTR_Code SetIgain(unsigned slotIdx,double gain);
- CTR_Code SetDgain(unsigned slotIdx,double gain);
- CTR_Code SetFgain(unsigned slotIdx,double gain);
- CTR_Code SetIzone(unsigned slotIdx,int zone);
- CTR_Code SetCloseLoopRampRate(unsigned slotIdx,int closeLoopRampRate);
- CTR_Code SetVoltageCompensationRate(double voltagePerMs);
- CTR_Code SetSensorPosition(int pos);
- CTR_Code SetForwardSoftLimit(int forwardLimit);
- CTR_Code SetReverseSoftLimit(int reverseLimit);
- CTR_Code SetForwardSoftEnable(int enable);
- CTR_Code SetReverseSoftEnable(int enable);
- CTR_Code GetPgain(unsigned slotIdx,double & gain);
- CTR_Code GetIgain(unsigned slotIdx,double & gain);
- CTR_Code GetDgain(unsigned slotIdx,double & gain);
- CTR_Code GetFgain(unsigned slotIdx,double & gain);
- CTR_Code GetIzone(unsigned slotIdx,int & zone);
- CTR_Code GetCloseLoopRampRate(unsigned slotIdx,int & closeLoopRampRate);
- CTR_Code GetVoltageCompensationRate(double & voltagePerMs);
- CTR_Code GetForwardSoftLimit(int & forwardLimit);
- CTR_Code GetReverseSoftLimit(int & reverseLimit);
- CTR_Code GetForwardSoftEnable(int & enable);
- CTR_Code GetReverseSoftEnable(int & enable);
- /**
- * Change the periodMs of a TALON's status frame. See kStatusFrame_* enums for what's available.
- */
- CTR_Code SetStatusFrameRate(unsigned frameEnum, unsigned periodMs);
- /**
- * Clear all sticky faults in TALON.
- */
- CTR_Code ClearStickyFaults();
- /*------------------------ auto generated. This API is optimal since it uses the fire-and-forget CAN interface ----------------------*/
- /*------------------------ These signals should cover the majority of all use cases. ----------------------------------*/
- 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 GetAppliedThrottle(int ¶m);
- CTR_Code GetCloseLoopErr(int ¶m);
- CTR_Code GetFeedbackDeviceSelect(int ¶m);
- CTR_Code GetModeSelect(int ¶m);
- CTR_Code GetLimitSwitchEn(int ¶m);
- CTR_Code GetLimitSwitchClosedFor(int ¶m);
- CTR_Code GetLimitSwitchClosedRev(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 SetDemand(int param);
- CTR_Code SetOverrideLimitSwitchEn(int param);
- CTR_Code SetFeedbackDeviceSelect(int param);
- CTR_Code SetRevMotDuringCloseLoopEn(int param);
- CTR_Code SetOverrideBrakeType(int param);
- CTR_Code SetModeSelect(int param);
- CTR_Code SetModeSelect(int modeSelect,int demand);
- CTR_Code SetProfileSlotSelect(int param);
- CTR_Code SetRampThrottle(int param);
- CTR_Code SetRevFeedbackSensor(int param);
- CTR_Code GetPulseWidthPosition(int ¶m);
- CTR_Code GetPulseWidthVelocity(int ¶m);
- CTR_Code GetPulseWidthRiseToFallUs(int ¶m);
- CTR_Code GetPulseWidthRiseToRiseUs(int ¶m);
- CTR_Code IsPulseWidthSensorPresent(int ¶m);
-};
-extern "C" {
- void *c_TalonSRX_Create(int deviceNumber, int controlPeriodMs);
- void c_TalonSRX_Destroy(void *handle);
- CTR_Code c_TalonSRX_SetParam(void *handle, int paramEnum, double value);
- CTR_Code c_TalonSRX_RequestParam(void *handle, int paramEnum);
- CTR_Code c_TalonSRX_GetParamResponse(void *handle, int paramEnum, double *value);
- CTR_Code c_TalonSRX_GetParamResponseInt32(void *handle, int paramEnum, int *value);
- CTR_Code c_TalonSRX_SetStatusFrameRate(void *handle, unsigned frameEnum, unsigned periodMs);
- CTR_Code c_TalonSRX_ClearStickyFaults(void *handle);
- CTR_Code c_TalonSRX_GetFault_OverTemp(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFault_UnderVoltage(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFault_ForLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFault_RevLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFault_HardwareFailure(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFault_ForSoftLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFault_RevSoftLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetStckyFault_OverTemp(void *handle, int *param);
- CTR_Code c_TalonSRX_GetStckyFault_UnderVoltage(void *handle, int *param);
- CTR_Code c_TalonSRX_GetStckyFault_ForLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetStckyFault_RevLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetStckyFault_ForSoftLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetStckyFault_RevSoftLim(void *handle, int *param);
- CTR_Code c_TalonSRX_GetAppliedThrottle(void *handle, int *param);
- CTR_Code c_TalonSRX_GetCloseLoopErr(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFeedbackDeviceSelect(void *handle, int *param);
- CTR_Code c_TalonSRX_GetModeSelect(void *handle, int *param);
- CTR_Code c_TalonSRX_GetLimitSwitchEn(void *handle, int *param);
- CTR_Code c_TalonSRX_GetLimitSwitchClosedFor(void *handle, int *param);
- CTR_Code c_TalonSRX_GetLimitSwitchClosedRev(void *handle, int *param);
- CTR_Code c_TalonSRX_GetSensorPosition(void *handle, int *param);
- CTR_Code c_TalonSRX_GetSensorVelocity(void *handle, int *param);
- CTR_Code c_TalonSRX_GetCurrent(void *handle, double *param);
- CTR_Code c_TalonSRX_GetBrakeIsEnabled(void *handle, int *param);
- CTR_Code c_TalonSRX_GetEncPosition(void *handle, int *param);
- CTR_Code c_TalonSRX_GetEncVel(void *handle, int *param);
- CTR_Code c_TalonSRX_GetEncIndexRiseEvents(void *handle, int *param);
- CTR_Code c_TalonSRX_GetQuadApin(void *handle, int *param);
- CTR_Code c_TalonSRX_GetQuadBpin(void *handle, int *param);
- CTR_Code c_TalonSRX_GetQuadIdxpin(void *handle, int *param);
- CTR_Code c_TalonSRX_GetAnalogInWithOv(void *handle, int *param);
- CTR_Code c_TalonSRX_GetAnalogInVel(void *handle, int *param);
- CTR_Code c_TalonSRX_GetTemp(void *handle, double *param);
- CTR_Code c_TalonSRX_GetBatteryV(void *handle, double *param);
- CTR_Code c_TalonSRX_GetResetCount(void *handle, int *param);
- CTR_Code c_TalonSRX_GetResetFlags(void *handle, int *param);
- CTR_Code c_TalonSRX_GetFirmVers(void *handle, int *param);
- CTR_Code c_TalonSRX_SetDemand(void *handle, int param);
- CTR_Code c_TalonSRX_SetOverrideLimitSwitchEn(void *handle, int param);
- CTR_Code c_TalonSRX_SetFeedbackDeviceSelect(void *handle, int param);
- CTR_Code c_TalonSRX_SetRevMotDuringCloseLoopEn(void *handle, int param);
- CTR_Code c_TalonSRX_SetOverrideBrakeType(void *handle, int param);
- CTR_Code c_TalonSRX_SetModeSelect(void *handle, int param);
- CTR_Code c_TalonSRX_SetModeSelect2(void *handle, int modeSelect, int demand);
- CTR_Code c_TalonSRX_SetProfileSlotSelect(void *handle, int param);
- CTR_Code c_TalonSRX_SetRampThrottle(void *handle, int param);
- CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param);
- CTR_Code c_TalonSRX_GetPulseWidthPosition(void *handle, int *param);
- CTR_Code c_TalonSRX_GetPulseWidthVelocity(void *handle, int *param);
- CTR_Code c_TalonSRX_GetPulseWidthRiseToFallUs(void *handle, int *param);
- CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param);
- CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param);
-}
-#endif
-
+/**
+ * @brief CAN TALON SRX driver.
+ *
+ * The TALON SRX is designed to instrument all runtime signals periodically.
+ * The default periods are chosen to support 16 TALONs with 10ms update rate
+ * for control (throttle or setpoint). However these can be overridden with
+ * SetStatusFrameRate. @see SetStatusFrameRate
+ * The getters for these unsolicited signals are auto generated at the bottom
+ * of this module.
+ *
+ * Likewise most control signals are sent periodically using the fire-and-forget
+ * CAN API. The setters for these unsolicited signals are auto generated at the
+ * bottom of this module.
+ *
+ * Signals that are not available in an unsolicited fashion are the Close Loop
+ * gains. For teams that have a single profile for their TALON close loop they
+ * can use either the webpage to configure their TALONs once or set the PIDF,
+ * Izone, CloseLoopRampRate, etc... once in the robot application. These
+ * parameters are saved to flash so once they are loaded in the TALON, they
+ * will persist through power cycles and mode changes.
+ *
+ * For teams that have one or two profiles to switch between, they can use the
+ * same strategy since there are two slots to choose from and the
+ * ProfileSlotSelect is periodically sent in the 10 ms control frame.
+ *
+ * For teams that require changing gains frequently, they can use the soliciting
+ * API to get and set those parameters. Most likely they will only need to set
+ * them in a periodic fashion as a function of what motion the application is
+ * attempting. If this API is used, be mindful of the CAN utilization reported
+ * in the driver station.
+ *
+ * If calling application has used the config routines to configure the
+ * selected feedback sensor, then all positions are measured in floating point
+ * precision rotations. All sensor velocities are specified in floating point
+ * precision RPM.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
+ * HOWEVER, if calling application has not called the config routine for
+ * selected feedback sensor, then all getters/setters for position/velocity use
+ * the native engineering units of the Talon SRX firm (just like in 2015).
+ * Signals explained below.
+ *
+ * Encoder position is measured in encoder edges. Every edge is counted
+ * (similar to roboRIO 4X mode). Analog position is 10 bits, meaning 1024
+ * ticks per rotation (0V => 3.3V). Use SetFeedbackDeviceSelect to select
+ * which sensor type you need. Once you do that you can use GetSensorPosition()
+ * and GetSensorVelocity(). These signals are updated on CANBus every 20ms (by
+ * default). If a relative sensor is selected, you can zero (or change the
+ * current value) using SetSensorPosition.
+ *
+ * Analog Input and quadrature position (and velocity) are also explicitly
+ * reported in GetEncPosition, GetEncVel, GetAnalogInWithOv, GetAnalogInVel.
+ * These signals are available all the time, regardless of what sensor is
+ * selected at a rate of 100ms. This allows easy instrumentation for "in the
+ * pits" checking of all sensors regardless of modeselect. The 100ms rate is
+ * overridable for teams who want to acquire sensor data for processing, not
+ * just instrumentation. Or just select the sensor using
+ * SetFeedbackDeviceSelect to get it at 20ms.
+ *
+ * Velocity is in position ticks / 100ms.
+ *
+ * All output units are in respect to duty cycle (throttle) which is -1023(full
+ * reverse) to +1023 (full forward). This includes demand (which specifies
+ * duty cycle when in duty cycle mode) and rampRamp, which is in throttle units
+ * per 10ms (if nonzero).
+ *
+ * Pos and velocity close loops are calc'd as
+ * err = target - posOrVel.
+ * iErr += err;
+ * if( (IZone!=0) and abs(err) > IZone)
+ * ClearIaccum()
+ * output = P X err + I X iErr + D X dErr + F X target
+ * dErr = err - lastErr
+ * P, I, and D gains are always positive. F can be negative.
+ * Motor direction can be reversed using SetRevMotDuringCloseLoopEn if
+ * sensor and motor are out of phase. Similarly feedback sensor can also be
+ * reversed (multiplied by -1) if you prefer the sensor to be inverted.
+ *
+ * P gain is specified in throttle per error tick. For example, a value of 102
+ * is ~9.9% (which is 102/1023) throttle per 1 ADC unit(10bit) or 1 quadrature
+ * encoder edge depending on selected sensor.
+ *
+ * I gain is specified in throttle per integrated error. For example, a value
+ * of 10 equates to ~0.99% (which is 10/1023) for each accumulated ADC unit
+ * (10 bit) or 1 quadrature encoder edge depending on selected sensor.
+ * Close loop and integral accumulator runs every 1ms.
+ *
+ * D gain is specified in throttle per derivative error. For example a value of
+ * 102 equates to ~9.9% (which is 102/1023) per change of 1 unit (ADC or
+ * encoder) per ms.
+ *
+ * I Zone is specified in the same units as sensor position (ADC units or
+ * quadrature edges). If pos/vel error is outside of this value, the
+ * integrated error will auto-clear...
+ * if( (IZone!=0) and abs(err) > IZone)
+ * ClearIaccum()
+ * ...this is very useful in preventing integral windup and is highly
+ * recommended if using full PID to keep stability low.
+ *
+ * CloseLoopRampRate is in throttle units per 1ms. Set to zero to disable
+ * ramping. Works the same as RampThrottle but only is in effect when a close
+ * loop mode and profile slot is selected.
+ *
+ * auto generated using spreadsheet and wpiclassgen.py
+ * @link https://docs.google.com/spreadsheets/d/1OU_ZV7fZLGYUQ-Uhc8sVAmUmWTlT8XBFYK8lfjg_tac/edit#gid=1766046967
+ */
+#ifndef CanTalonSRX_H_
+#define CanTalonSRX_H_
+#include "ctre/ctre.h" //BIT Defines + Typedefs, TALON_Control_6_MotProfAddTrajPoint_t
+#include "ctre/CtreCanNode.h"
+#include <FRC_NetworkCommunication/CANSessionMux.h> //CAN Comm
+#include <map>
+#include <atomic>
+#include <deque>
+#include <mutex>
+class CanTalonSRX : public CtreCanNode {
+ private:
+ // Use this for determining whether the default move constructor has been
+ // called; this prevents us from calling the destructor twice.
+ struct HasBeenMoved {
+ HasBeenMoved(HasBeenMoved &&other) {
+ other.moved = true;
+ moved = false;
+ }
+ HasBeenMoved() = default;
+ std::atomic<bool> moved{false};
+ operator bool() const { return moved; }
+ } m_hasBeenMoved;
+
+ // Vars for opening a CAN stream if caller needs signals that require
+ // soliciting
+ uint32_t _can_h; //!< Session handle for catching response params.
+ int32_t _can_stat; //!< Session handle status.
+ struct tCANStreamMessage _msgBuff[20];
+ static int const kMsgCapacity = 20;
+ typedef std::map<uint32_t, uint32_t> sigs_t;
+ // Catches signal updates that are solicited. Expect this to be very few.
+ sigs_t _sigs;
+ void OpenSessionIfNeedBe();
+ void ProcessStreamMessages();
+ /**
+ * Called in various places to double check we are using the best control
+ * frame. If the Talon firmware is too old, use control 1 framing, which
+ * does not allow setting control signals until robot is enabled. If Talon
+ * firmware can suport control5, use that since that frame can be
+ * transmitted during robot-disable. If calling application uses setParam
+ * to set the signal eLegacyControlMode, caller can force using control1
+ * if needed for some reason.
+ */
+ void UpdateControlId();
+ /**
+ * @return true if Talon is reporting that it supports control5, and therefore
+ * RIO can send control5 to update control params (even when
+ * disabled).
+ */
+ bool IsControl5Supported();
+ /**
+ * Get a copy of the control frame to send.
+ * @param [out] pointer to eight byte array to fill.
+ */
+ void GetControlFrameCopy(uint8_t *toFill);
+ /**
+ * @return the tx task that transmits Control6 (motion profile control).
+ * If it's not scheduled, then schedule it. This is part
+ * of making the lazy-framing that only peforms MotionProf framing
+ * when needed to save bandwidth.
+ */
+ CtreCanNode::txTask<TALON_Control_6_MotProfAddTrajPoint_t> GetControl6();
+ /**
+ * Caller is either pushing a new motion profile point, or is
+ * calling the Process buffer routine. In either case check our
+ * flow control to see if we need to start sending control6.
+ */
+ void ReactToMotionProfileCall();
+ /**
+ * Update the NextPt signals inside the control frame given the next pt to
+ * send.
+ * @param control pointer to the CAN frame payload containing control6. Only
+ * the signals that serialize the next trajectory point are
+ * updated from the contents of newPt.
+ * @param newPt point to the next trajectory that needs to be inserted into
+ * Talon RAM.
+ */
+ void CopyTrajPtIntoControl(
+ TALON_Control_6_MotProfAddTrajPoint_t *control,
+ const TALON_Control_6_MotProfAddTrajPoint_t *newPt);
+ //---------------------- General Control framing ---------------------------//
+ /**
+ * Frame period for control1 or control5, depending on which one we are using.
+ */
+ int _controlPeriodMs = kDefaultControlPeriodMs;
+ /**
+ * Frame Period of the motion profile control6 frame.
+ */
+ int _control6PeriodMs = kDefaultControl6PeriodMs;
+ /**
+ * When using control5, we still need to send a frame to enable robot. This
+ * controls the period. This only is used when we are in the control5 state.
+ * @see ControlFrameSelControl5
+ */
+ int _enablePeriodMs = kDefaultEnablePeriodMs;
+ /**
+ * ArbID to use for control frame. Should be either CONTROL_1 or CONTROL_5.
+ */
+ uint32_t _controlFrameArbId;
+ /**
+ * Boolean flag to signal calling applications intent to allow using control5
+ * assuming Talon firmware supports it. This can be cleared to force control1
+ * framing.
+ */
+ bool _useControl5ifSupported = true;
+ //--------------------- Buffering Motion Profile ---------------------------//
+ /**
+ * Top level Buffer for motion profile trajectory buffering.
+ * Basically this buffers up the eight byte CAN frame payloads that are
+ * handshaked into the Talon RAM.
+ * TODO: Should this be moved into a separate header, and if so where
+ * logically should it reside?
+ * TODO: Add compression so that multiple CAN frames can be compressed into
+ * one exchange.
+ */
+ class TrajectoryBuffer {
+ public:
+ void Clear() { _motProfTopBuffer.clear(); }
+ /**
+ * push caller's uncompressed simple trajectory point.
+ */
+ void Push(TALON_Control_6_MotProfAddTrajPoint_huff0_t &pt) {
+ _motProfTopBuffer.push_back(pt);
+ }
+ /**
+ * Get the next trajectory point CAN frame to send.
+ * Underlying layer may compress the next few points together
+ * into one control_6 frame.
+ */
+ TALON_Control_6_MotProfAddTrajPoint_t *Front() {
+ /* TODO : peek ahead and use compression strategies */
+ _lastFront = _motProfTopBuffer.front();
+ return (TALON_Control_6_MotProfAddTrajPoint_t *)&_lastFront;
+ }
+ void Pop() {
+ /* TODO : pop multiple points if last front'd point was compressed. */
+ _motProfTopBuffer.pop_front();
+ }
+ unsigned int GetNumTrajectories() { return _motProfTopBuffer.size(); }
+ bool IsEmpty() { return _motProfTopBuffer.empty(); }
+
+ private:
+ std::deque<TALON_Control_6_MotProfAddTrajPoint_huff0_t> _motProfTopBuffer;
+ TALON_Control_6_MotProfAddTrajPoint_huff0_t _lastFront;
+ };
+ TrajectoryBuffer _motProfTopBuffer;
+ /**
+ * To keep buffers from getting out of control, place a cap on the top level
+ * buffer. Calling application
+ * can stream addition points as they are fed to Talon.
+ * Approx memory footprint is this capacity X 8 bytes.
+ */
+ static const int kMotionProfileTopBufferCapacity = 2048;
+ /**
+ * Flow control for streaming trajectories.
+ */
+ int32_t _motProfFlowControl = -1;
+ /**
+ * Since we may need the MP pts to be emptied into Talon in the background
+ * make sure the buffering is thread-safe.
+ */
+ std::mutex _mutMotProf;
+ /**
+ * Send a one shot frame to set an arbitrary signal.
+ * Most signals are in the control frame so avoid using this API unless you
+ * have to.
+ * Use this api for...
+ * -A motor controller profile signal eProfileParam_XXXs. These are backed up
+ * in flash. If you are gain-scheduling then call this periodically.
+ * -Default brake and limit switch signals... eOnBoot_XXXs. Avoid doing this,
+ * use the override signals in the control frame.
+ * Talon will automatically send a PARAM_RESPONSE after the set, so
+ * GetParamResponse will catch the latest value after a couple ms.
+ */
+ CTR_Code SetParamRaw(uint32_t paramEnum, int32_t rawBits);
+ /**
+ * Checks cached CAN frames and updating solicited signals.
+ */
+ CTR_Code GetParamResponseRaw(uint32_t paramEnum, int32_t &rawBits);
+
+ public:
+ // default control update rate is 10ms.
+ static const int kDefaultControlPeriodMs = 10;
+ // default enable update rate is 50ms (when using the new control5 frame).
+ static const int kDefaultEnablePeriodMs = 50;
+ // Default update rate for motion profile control 6. This only takes effect
+ // when calling uses MP functions.
+ static const int kDefaultControl6PeriodMs = 10;
+ explicit CanTalonSRX(int deviceNumber = 0,
+ int controlPeriodMs = kDefaultControlPeriodMs,
+ int enablePeriodMs = kDefaultEnablePeriodMs);
+ ~CanTalonSRX();
+ void Set(double value);
+ /* mode select enumerations */
+ // Demand is 11bit signed duty cycle [-1023,1023].
+ static const int kMode_DutyCycle = 0;
+ // Position PIDF.
+ static const int kMode_PositionCloseLoop = 1;
+ // Velocity PIDF.
+ static const int kMode_VelocityCloseLoop = 2;
+ // Current close loop - not done.
+ static const int kMode_CurrentCloseLoop = 3;
+ // Voltage Compensation Mode - not done. Demand is fixed pt target 8.8 volts.
+ static const int kMode_VoltCompen = 4;
+ // Demand is the 6 bit Device ID of the 'master' TALON SRX.
+ static const int kMode_SlaveFollower = 5;
+ // Demand is '0' (Disabled), '1' (Enabled), or '2' (Hold).
+ static const int kMode_MotionProfile = 6;
+ // Zero the output (honors brake/coast) regardless of demand.
+ // Might be useful if we need to change modes but can't atomically
+ // change all the signals we want in between.
+ static const int kMode_NoDrive = 15;
+ /* limit switch enumerations */
+ static const int kLimitSwitchOverride_UseDefaultsFromFlash = 1;
+ static const int kLimitSwitchOverride_DisableFwd_DisableRev = 4;
+ static const int kLimitSwitchOverride_DisableFwd_EnableRev = 5;
+ static const int kLimitSwitchOverride_EnableFwd_DisableRev = 6;
+ static const int kLimitSwitchOverride_EnableFwd_EnableRev = 7;
+ /* brake override enumerations */
+ static const int kBrakeOverride_UseDefaultsFromFlash = 0;
+ static const int kBrakeOverride_OverrideCoast = 1;
+ static const int kBrakeOverride_OverrideBrake = 2;
+ /* feedback device enumerations */
+ static const int kFeedbackDev_DigitalQuadEnc = 0;
+ static const int kFeedbackDev_AnalogPot = 2;
+ static const int kFeedbackDev_AnalogEncoder = 3;
+ static const int kFeedbackDev_CountEveryRisingEdge = 4;
+ static const int kFeedbackDev_CountEveryFallingEdge = 5;
+ static const int kFeedbackDev_PosIsPulseWidth = 8;
+ /* ProfileSlotSelect enumerations*/
+ static const int kProfileSlotSelect_Slot0 = 0;
+ static const int kProfileSlotSelect_Slot1 = 1;
+ /* status frame rate types */
+ static const int kStatusFrame_General = 0;
+ static const int kStatusFrame_Feedback = 1;
+ static const int kStatusFrame_Encoder = 2;
+ static const int kStatusFrame_AnalogTempVbat = 3;
+ static const int kStatusFrame_PulseWidthMeas = 4;
+ static const int kStatusFrame_MotionProfile = 5;
+ /* Motion Profile status bits */
+ static const int kMotionProfileFlag_ActTraj_IsValid = 0x1;
+ static const int kMotionProfileFlag_HasUnderrun = 0x2;
+ static const int kMotionProfileFlag_IsUnderrun = 0x4;
+ static const int kMotionProfileFlag_ActTraj_IsLast = 0x8;
+ static const int kMotionProfileFlag_ActTraj_VelOnly = 0x10;
+ /* Motion Profile Set Output */
+ // Motor output is neutral, Motion Profile Executer is not running.
+ static const int kMotionProf_Disabled = 0;
+ // Motor output is updated from Motion Profile Executer, MPE will
+ // process the buffered points.
+ static const int kMotionProf_Enable = 1;
+ // Motor output is updated from Motion Profile Executer, MPE will
+ // stay processing current trajectory point.
+ static const int kMotionProf_Hold = 2;
+ /**
+ * Signal enumeration for generic signal access.
+ * Although every signal is enumerated, only use this for traffic that must
+ * be solicited.
+ * Use the auto generated getters/setters at bottom of this header as much as
+ * possible.
+ */
+ enum param_t {
+ eProfileParamSlot0_P = 1,
+ eProfileParamSlot0_I = 2,
+ eProfileParamSlot0_D = 3,
+ eProfileParamSlot0_F = 4,
+ eProfileParamSlot0_IZone = 5,
+ eProfileParamSlot0_CloseLoopRampRate = 6,
+ eProfileParamSlot1_P = 11,
+ eProfileParamSlot1_I = 12,
+ eProfileParamSlot1_D = 13,
+ eProfileParamSlot1_F = 14,
+ eProfileParamSlot1_IZone = 15,
+ eProfileParamSlot1_CloseLoopRampRate = 16,
+ eProfileParamSoftLimitForThreshold = 21,
+ eProfileParamSoftLimitRevThreshold = 22,
+ eProfileParamSoftLimitForEnable = 23,
+ eProfileParamSoftLimitRevEnable = 24,
+ eOnBoot_BrakeMode = 31,
+ eOnBoot_LimitSwitch_Forward_NormallyClosed = 32,
+ eOnBoot_LimitSwitch_Reverse_NormallyClosed = 33,
+ eOnBoot_LimitSwitch_Forward_Disable = 34,
+ eOnBoot_LimitSwitch_Reverse_Disable = 35,
+ eFault_OverTemp = 41,
+ eFault_UnderVoltage = 42,
+ eFault_ForLim = 43,
+ eFault_RevLim = 44,
+ eFault_HardwareFailure = 45,
+ eFault_ForSoftLim = 46,
+ eFault_RevSoftLim = 47,
+ eStckyFault_OverTemp = 48,
+ eStckyFault_UnderVoltage = 49,
+ eStckyFault_ForLim = 50,
+ eStckyFault_RevLim = 51,
+ eStckyFault_ForSoftLim = 52,
+ eStckyFault_RevSoftLim = 53,
+ eAppliedThrottle = 61,
+ eCloseLoopErr = 62,
+ eFeedbackDeviceSelect = 63,
+ eRevMotDuringCloseLoopEn = 64,
+ eModeSelect = 65,
+ eProfileSlotSelect = 66,
+ eRampThrottle = 67,
+ eRevFeedbackSensor = 68,
+ eLimitSwitchEn = 69,
+ eLimitSwitchClosedFor = 70,
+ eLimitSwitchClosedRev = 71,
+ eSensorPosition = 73,
+ eSensorVelocity = 74,
+ eCurrent = 75,
+ eBrakeIsEnabled = 76,
+ eEncPosition = 77,
+ eEncVel = 78,
+ eEncIndexRiseEvents = 79,
+ eQuadApin = 80,
+ eQuadBpin = 81,
+ eQuadIdxpin = 82,
+ eAnalogInWithOv = 83,
+ eAnalogInVel = 84,
+ eTemp = 85,
+ eBatteryV = 86,
+ eResetCount = 87,
+ eResetFlags = 88,
+ eFirmVers = 89,
+ eSettingsChanged = 90,
+ eQuadFilterEn = 91,
+ ePidIaccum = 93,
+ eStatus1FrameRate = 94, // TALON_Status_1_General_10ms_t
+ eStatus2FrameRate = 95, // TALON_Status_2_Feedback_20ms_t
+ eStatus3FrameRate = 96, // TALON_Status_3_Enc_100ms_t
+ eStatus4FrameRate = 97, // TALON_Status_4_AinTempVbat_100ms_t
+ eStatus6FrameRate = 98, // TALON_Status_6_Eol_t
+ eStatus7FrameRate = 99, // TALON_Status_7_Debug_200ms_t
+ eClearPositionOnIdx = 100,
+ // reserved,
+ // reserved,
+ // reserved,
+ ePeakPosOutput = 104,
+ eNominalPosOutput = 105,
+ ePeakNegOutput = 106,
+ eNominalNegOutput = 107,
+ eQuadIdxPolarity = 108,
+ eStatus8FrameRate = 109, // TALON_Status_8_PulseWid_100ms_t
+ eAllowPosOverflow = 110,
+ eProfileParamSlot0_AllowableClosedLoopErr = 111,
+ eNumberPotTurns = 112,
+ eNumberEncoderCPR = 113,
+ ePwdPosition = 114,
+ eAinPosition = 115,
+ eProfileParamVcompRate = 116,
+ eProfileParamSlot1_AllowableClosedLoopErr = 117,
+ eStatus9FrameRate = 118, // TALON_Status_9_MotProfBuffer_100ms_t
+ eMotionProfileHasUnderrunErr = 119,
+ eReserved120 = 120,
+ eLegacyControlMode = 121,
+ };
+ //---- setters and getters that use the solicated param request/response ---//
+ /**
+ * Send a one shot frame to set an arbitrary signal.
+ * Most signals are in the control frame so avoid using this API unless you
+ * have to.
+ * Use this api for...
+ * -A motor controller profile signal eProfileParam_XXXs. These are backed
+ * up in flash. If you are gain-scheduling then call this periodically.
+ * -Default brake and limit switch signals... eOnBoot_XXXs. Avoid doing
+ * this, use the override signals in the control frame.
+ * Talon will automatically send a PARAM_RESPONSE after the set, so
+ * GetParamResponse will catch the latest value after a couple ms.
+ */
+ CTR_Code SetParam(param_t paramEnum, double value);
+ /**
+ * Asks TALON to immedietely respond with signal value. This API is only used
+ * for signals that are not sent periodically.
+ * This can be useful for reading params that rarely change like Limit Switch
+ * settings and PIDF values.
+ * @param param to request.
+ */
+ CTR_Code RequestParam(param_t paramEnum);
+ CTR_Code GetParamResponse(param_t paramEnum, double &value);
+ CTR_Code GetParamResponseInt32(param_t paramEnum, int &value);
+ //----------- getters and setters that use param request/response ----------//
+ /**
+ * These signals are backed up in flash and will survive a power cycle.
+ * If your application requires changing these values consider using both
+ * slots and switch between slot0 <=> slot1.
+ * If your application requires changing these signals frequently then it
+ * makes sense to leverage this API.
+ * Getters don't block, so it may require several calls to get the latest
+ * value.
+ */
+ CTR_Code SetPgain(unsigned slotIdx, double gain);
+ CTR_Code SetIgain(unsigned slotIdx, double gain);
+ CTR_Code SetDgain(unsigned slotIdx, double gain);
+ CTR_Code SetFgain(unsigned slotIdx, double gain);
+ CTR_Code SetIzone(unsigned slotIdx, int zone);
+ CTR_Code SetCloseLoopRampRate(unsigned slotIdx, int closeLoopRampRate);
+ CTR_Code SetVoltageCompensationRate(double voltagePerMs);
+ CTR_Code SetSensorPosition(int pos);
+ CTR_Code SetForwardSoftLimit(int forwardLimit);
+ CTR_Code SetReverseSoftLimit(int reverseLimit);
+ CTR_Code SetForwardSoftEnable(int enable);
+ CTR_Code SetReverseSoftEnable(int enable);
+ CTR_Code GetPgain(unsigned slotIdx, double &gain);
+ CTR_Code GetIgain(unsigned slotIdx, double &gain);
+ CTR_Code GetDgain(unsigned slotIdx, double &gain);
+ CTR_Code GetFgain(unsigned slotIdx, double &gain);
+ CTR_Code GetIzone(unsigned slotIdx, int &zone);
+ CTR_Code GetCloseLoopRampRate(unsigned slotIdx, int &closeLoopRampRate);
+ CTR_Code GetVoltageCompensationRate(double &voltagePerMs);
+ CTR_Code GetForwardSoftLimit(int &forwardLimit);
+ CTR_Code GetReverseSoftLimit(int &reverseLimit);
+ CTR_Code GetForwardSoftEnable(int &enable);
+ CTR_Code GetReverseSoftEnable(int &enable);
+ CTR_Code GetPulseWidthRiseToFallUs(int ¶m);
+ CTR_Code IsPulseWidthSensorPresent(int ¶m);
+ CTR_Code SetModeSelect(int modeSelect, int demand);
+ /**
+ * Change the periodMs of a TALON's status frame. See kStatusFrame_* enums
+ * for what's available.
+ */
+ CTR_Code SetStatusFrameRate(unsigned frameEnum, unsigned periodMs);
+ /**
+ * Clear all sticky faults in TALON.
+ */
+ CTR_Code ClearStickyFaults();
+ /**
+ * Calling application can opt to speed up the handshaking between the robot
+ * API and the Talon to increase the
+ * download rate of the Talon's Motion Profile. Ideally the period should be
+ * no more than half the period
+ * of a trajectory point.
+ */
+ void ChangeMotionControlFramePeriod(uint32_t periodMs);
+ /**
+ * Clear the buffered motion profile in both Talon RAM (bottom), and in the
+ * API (top).
+ */
+ void ClearMotionProfileTrajectories();
+ /**
+ * Retrieve just the buffer count for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and
+ * ideal if caller needs to quickly poll the progress of trajectory points
+ * being emptied into Talon's RAM. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+ uint32_t GetMotionProfileTopLevelBufferCount();
+ /**
+ * Retrieve just the buffer full for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and
+ * ideal if caller needs to quickly poll. Otherwise just use
+ * GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+ bool IsMotionProfileTopLevelBufferFull();
+ /**
+ * Push another trajectory point into the top level buffer (which is emptied
+ * into the Talon's bottom buffer as room allows).
+ * @param targPos servo position in native Talon units (sensor units).
+ * @param targVel velocity to feed-forward in native Talon units (sensor
+ * units per 100ms).
+ * @param profileSlotSelect which slot to pull PIDF gains from. Currently
+ * supports 0 or 1.
+ * @param timeDurMs time in milliseconds of how long to apply this point.
+ * @param velOnly set to nonzero to signal Talon that only the feed-foward
+ * velocity should be used, i.e. do not perform PID on
+ * position. This is equivalent to setting PID gains to zero,
+ * but much more efficient and synchronized to MP.
+ * @param isLastPoint set to nonzero to signal Talon to keep processing this
+ * trajectory point, instead of jumping to the next one
+ * when timeDurMs expires. Otherwise MP executer will
+ * eventually see an empty buffer after the last point
+ * expires, causing it to assert the IsUnderRun flag.
+ * However this may be desired if calling application
+ * nevers wants to terminate the MP.
+ * @param zeroPos set to nonzero to signal Talon to "zero" the selected
+ * position sensor before executing this trajectory point.
+ * Typically the first point should have this set only thus
+ * allowing the remainder of the MP positions to be relative
+ * to zero.
+ * @return CTR_OKAY if trajectory point push ok. CTR_BufferFull if buffer is
+ * full due to kMotionProfileTopBufferCapacity.
+ */
+ CTR_Code PushMotionProfileTrajectory(int targPos, int targVel,
+ int profileSlotSelect, int timeDurMs,
+ int velOnly, int isLastPoint,
+ int zeroPos);
+ /**
+ * This must be called periodically to funnel the trajectory points from the
+ * API's top level buffer to the Talon's bottom level buffer. Recommendation
+ * is to call this twice as fast as the executation rate of the motion
+ * profile. So if MP is running with 20ms trajectory points, try calling
+ * this routine every 10ms. All motion profile functions are thread-safe
+ * through the use of a mutex, so there is no harm in having the caller
+ * utilize threading.
+ */
+ void ProcessMotionProfileBuffer();
+ /**
+ * Retrieve all status information.
+ * Since this all comes from one CAN frame, its ideal to have one routine to
+ * retrieve the frame once and decode everything.
+ * @param [out] flags bitfield for status bools. Starting with least
+ * significant bit: IsValid, HasUnderrun, IsUnderrun, IsLast, VelOnly.
+ *
+ * IsValid set when MP executer is processing a trajectory point,
+ * and that point's status is instrumented with IsLast,
+ * VelOnly, targPos, targVel. However if MP executor is
+ * not processing a trajectory point, then this flag is
+ * false, and the instrumented signals will be zero.
+ * HasUnderrun is set anytime the MP executer is ready to pop
+ * another trajectory point from the Talon's RAM,
+ * but the buffer is empty. It can only be cleared
+ * by using SetParam(eMotionProfileHasUnderrunErr,0);
+ * IsUnderrun is set when the MP executer is ready for another
+ * point, but the buffer is empty, and cleared when
+ * the MP executer does not need another point.
+ * HasUnderrun shadows this registor when this
+ * register gets set, however HasUnderrun stays
+ * asserted until application has process it, and
+ * IsUnderrun auto-clears when the condition is
+ * resolved.
+ * IsLast is set/cleared based on the MP executer's current
+ * trajectory point's IsLast value. This assumes
+ * IsLast was set when PushMotionProfileTrajectory
+ * was used to insert the currently processed trajectory
+ * point.
+ * VelOnly is set/cleared based on the MP executer's current
+ * trajectory point's VelOnly value.
+ *
+ * @param [out] profileSlotSelect The currently processed trajectory point's
+ * selected slot. This can differ in the currently selected slot used
+ * for Position and Velocity servo modes.
+ * @param [out] targPos The currently processed trajectory point's position
+ * in native units. This param is zero if IsValid is zero.
+ * @param [out] targVel The currently processed trajectory point's velocity
+ * in native units. This param is zero if IsValid is zero.
+ * @param [out] topBufferRem The remaining number of points in the top level
+ * buffer.
+ * @param [out] topBufferCnt The number of points in the top level buffer to
+ * be sent to Talon.
+ * @param [out] btmBufferCnt The number of points in the bottom level buffer
+ * inside Talon.
+ * @return CTR error code
+ */
+ CTR_Code GetMotionProfileStatus(uint32_t &flags, uint32_t &profileSlotSelect,
+ int32_t &targPos, int32_t &targVel,
+ uint32_t &topBufferRemaining,
+ uint32_t &topBufferCnt,
+ uint32_t &btmBufferCnt,
+ uint32_t &outputEnable);
+//------------------------ auto generated ------------------------------------//
+/* This API is optimal since it uses the fire-and-forget CAN interface.
+ * These signals should cover the majority of all use cases.
+ */
+ 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 GetAppliedThrottle(int ¶m);
+ CTR_Code GetCloseLoopErr(int ¶m);
+ CTR_Code GetFeedbackDeviceSelect(int ¶m);
+ CTR_Code GetModeSelect(int ¶m);
+ CTR_Code GetLimitSwitchEn(int ¶m);
+ CTR_Code GetLimitSwitchClosedFor(int ¶m);
+ CTR_Code GetLimitSwitchClosedRev(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 GetPulseWidthPosition(int ¶m);
+ CTR_Code GetPulseWidthVelocity(int ¶m);
+ CTR_Code GetPulseWidthRiseToRiseUs(int ¶m);
+ CTR_Code GetActTraj_IsValid(int ¶m);
+ CTR_Code GetActTraj_ProfileSlotSelect(int ¶m);
+ CTR_Code GetActTraj_VelOnly(int ¶m);
+ CTR_Code GetActTraj_IsLast(int ¶m);
+ CTR_Code GetOutputType(int ¶m);
+ CTR_Code GetHasUnderrun(int ¶m);
+ CTR_Code GetIsUnderrun(int ¶m);
+ CTR_Code GetNextID(int ¶m);
+ CTR_Code GetBufferIsFull(int ¶m);
+ CTR_Code GetCount(int ¶m);
+ CTR_Code GetActTraj_Velocity(int ¶m);
+ CTR_Code GetActTraj_Position(int ¶m);
+ CTR_Code SetDemand(int param);
+ CTR_Code SetOverrideLimitSwitchEn(int param);
+ CTR_Code SetFeedbackDeviceSelect(int param);
+ CTR_Code SetRevMotDuringCloseLoopEn(int param);
+ CTR_Code SetOverrideBrakeType(int param);
+ CTR_Code SetModeSelect(int param);
+ CTR_Code SetProfileSlotSelect(int param);
+ CTR_Code SetRampThrottle(int param);
+ CTR_Code SetRevFeedbackSensor(int param);
+};
+extern "C" {
+ void *c_TalonSRX_Create3(int deviceNumber, int controlPeriodMs, int enablePeriodMs);
+ void *c_TalonSRX_Create2(int deviceNumber, int controlPeriodMs);
+ void *c_TalonSRX_Create1(int deviceNumber);
+ void c_TalonSRX_Destroy(void *handle);
+ void c_TalonSRX_Set(void *handle, double value);
+ CTR_Code c_TalonSRX_SetParam(void *handle, int paramEnum, double value);
+ CTR_Code c_TalonSRX_RequestParam(void *handle, int paramEnum);
+ CTR_Code c_TalonSRX_GetParamResponse(void *handle, int paramEnum, double *value);
+ CTR_Code c_TalonSRX_GetParamResponseInt32(void *handle, int paramEnum, int *value);
+ CTR_Code c_TalonSRX_SetPgain(void *handle, int slotIdx, double gain);
+ CTR_Code c_TalonSRX_SetIgain(void *handle, int slotIdx, double gain);
+ CTR_Code c_TalonSRX_SetDgain(void *handle, int slotIdx, double gain);
+ CTR_Code c_TalonSRX_SetFgain(void *handle, int slotIdx, double gain);
+ CTR_Code c_TalonSRX_SetIzone(void *handle, int slotIdx, int zone);
+ CTR_Code c_TalonSRX_SetCloseLoopRampRate(void *handle, int slotIdx, int closeLoopRampRate);
+ CTR_Code c_TalonSRX_SetVoltageCompensationRate(void *handle, double voltagePerMs);
+ CTR_Code c_TalonSRX_SetSensorPosition(void *handle, int pos);
+ CTR_Code c_TalonSRX_SetForwardSoftLimit(void *handle, int forwardLimit);
+ CTR_Code c_TalonSRX_SetReverseSoftLimit(void *handle, int reverseLimit);
+ CTR_Code c_TalonSRX_SetForwardSoftEnable(void *handle, int enable);
+ CTR_Code c_TalonSRX_SetReverseSoftEnable(void *handle, int enable);
+ CTR_Code c_TalonSRX_GetPgain(void *handle, int slotIdx, double *gain);
+ CTR_Code c_TalonSRX_GetIgain(void *handle, int slotIdx, double *gain);
+ CTR_Code c_TalonSRX_GetDgain(void *handle, int slotIdx, double *gain);
+ CTR_Code c_TalonSRX_GetFgain(void *handle, int slotIdx, double *gain);
+ CTR_Code c_TalonSRX_GetIzone(void *handle, int slotIdx, int *zone);
+ CTR_Code c_TalonSRX_GetCloseLoopRampRate(void *handle, int slotIdx, int *closeLoopRampRate);
+ CTR_Code c_TalonSRX_GetVoltageCompensationRate(void *handle, double *voltagePerMs);
+ CTR_Code c_TalonSRX_GetForwardSoftLimit(void *handle, int *forwardLimit);
+ CTR_Code c_TalonSRX_GetReverseSoftLimit(void *handle, int *reverseLimit);
+ CTR_Code c_TalonSRX_GetForwardSoftEnable(void *handle, int *enable);
+ CTR_Code c_TalonSRX_GetReverseSoftEnable(void *handle, int *enable);
+ CTR_Code c_TalonSRX_GetPulseWidthRiseToFallUs(void *handle, int *param);
+ CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param);
+ CTR_Code c_TalonSRX_SetModeSelect2(void *handle, int modeSelect, int demand);
+ CTR_Code c_TalonSRX_SetStatusFrameRate(void *handle, int frameEnum, int periodMs);
+ CTR_Code c_TalonSRX_ClearStickyFaults(void *handle);
+ void c_TalonSRX_ChangeMotionControlFramePeriod(void *handle, int periodMs);
+ void c_TalonSRX_ClearMotionProfileTrajectories(void *handle);
+ int c_TalonSRX_GetMotionProfileTopLevelBufferCount(void *handle);
+ int c_TalonSRX_IsMotionProfileTopLevelBufferFull(void *handle);
+ CTR_Code c_TalonSRX_PushMotionProfileTrajectory(void *handle, int targPos, int targVel, int profileSlotSelect, int timeDurMs, int velOnly, int isLastPoint, int zeroPos);
+ void c_TalonSRX_ProcessMotionProfileBuffer(void *handle);
+ CTR_Code c_TalonSRX_GetMotionProfileStatus(void *handle, int *flags, int *profileSlotSelect, int *targPos, int *targVel, int *topBufferRemaining, int *topBufferCnt, int *btmBufferCnt, int *outputEnable);
+ CTR_Code c_TalonSRX_GetFault_OverTemp(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_UnderVoltage(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_ForLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_RevLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_HardwareFailure(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_ForSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_RevSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_OverTemp(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_UnderVoltage(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_ForLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_RevLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_ForSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_RevSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetAppliedThrottle(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetCloseLoopErr(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFeedbackDeviceSelect(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetModeSelect(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetLimitSwitchEn(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetLimitSwitchClosedFor(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetLimitSwitchClosedRev(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetSensorPosition(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetSensorVelocity(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetCurrent(void *handle, double *param);
+ CTR_Code c_TalonSRX_GetBrakeIsEnabled(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetEncPosition(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetEncVel(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetEncIndexRiseEvents(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetQuadApin(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetQuadBpin(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetQuadIdxpin(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetAnalogInWithOv(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetAnalogInVel(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetTemp(void *handle, double *param);
+ CTR_Code c_TalonSRX_GetBatteryV(void *handle, double *param);
+ CTR_Code c_TalonSRX_GetResetCount(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetResetFlags(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFirmVers(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetPulseWidthPosition(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetPulseWidthVelocity(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetActTraj_IsValid(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetActTraj_ProfileSlotSelect(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetActTraj_VelOnly(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetActTraj_IsLast(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetOutputType(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetHasUnderrun(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetIsUnderrun(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetNextID(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetBufferIsFull(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetCount(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetActTraj_Velocity(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetActTraj_Position(void *handle, int *param);
+ CTR_Code c_TalonSRX_SetDemand(void *handle, int param);
+ CTR_Code c_TalonSRX_SetOverrideLimitSwitchEn(void *handle, int param);
+ CTR_Code c_TalonSRX_SetFeedbackDeviceSelect(void *handle, int param);
+ CTR_Code c_TalonSRX_SetRevMotDuringCloseLoopEn(void *handle, int param);
+ CTR_Code c_TalonSRX_SetOverrideBrakeType(void *handle, int param);
+ CTR_Code c_TalonSRX_SetModeSelect(void *handle, int param);
+ CTR_Code c_TalonSRX_SetProfileSlotSelect(void *handle, int param);
+ CTR_Code c_TalonSRX_SetRampThrottle(void *handle, int param);
+ CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param);
+}
+#endif
diff --git a/hal/include/HAL/HAL.hpp b/hal/include/HAL/HAL.hpp
index f09a5d2..fb3c6aa 100644
--- a/hal/include/HAL/HAL.hpp
+++ b/hal/include/HAL/HAL.hpp
@@ -222,11 +222,14 @@
uint16_t getFPGAVersion(int32_t *status);
uint32_t getFPGARevision(int32_t *status);
- uint32_t getFPGATime(int32_t *status);
+ uint64_t getFPGATime(int32_t *status);
bool getFPGAButton(int32_t *status);
int HALSetErrorData(const char *errors, int errorsLength, int wait_ms);
+ int HALSendError(int isError, int32_t errorCode, int isLVCode,
+ const char *details, const char *location, const char *callStack,
+ int printMsg);
int HALGetControlWord(HALControlWord *data);
int HALGetAllianceStation(enum HALAllianceStationID *allianceStation);
diff --git a/hal/include/HAL/Notifier.hpp b/hal/include/HAL/Notifier.hpp
index f16572f..e8cb381 100644
--- a/hal/include/HAL/Notifier.hpp
+++ b/hal/include/HAL/Notifier.hpp
@@ -4,8 +4,9 @@
extern "C"
{
- void* initializeNotifier(void (*ProcessQueue)(uint32_t, void*), void* param, int32_t *status);
+ void* initializeNotifier(void (*process)(uint64_t, void*), void* param, int32_t *status);
void cleanNotifier(void* notifier_pointer, int32_t *status);
-
- void updateNotifierAlarm(void* notifier_pointer, uint32_t triggerTime, int32_t *status);
+ void* getNotifierParam(void* notifier_pointer, int32_t *status);
+ void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, int32_t *status);
+ void stopNotifierAlarm(void* notifier_pointer, int32_t *status);
}
diff --git a/hal/include/HAL/Port.h b/hal/include/HAL/Port.h
index 8679c26..b2e97c2 100644
--- a/hal/include/HAL/Port.h
+++ b/hal/include/HAL/Port.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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. */
+/*----------------------------------------------------------------------------*/
+
#pragma once
typedef struct port_t
diff --git a/hal/include/HAL/cpp/priority_condition_variable.h b/hal/include/HAL/cpp/priority_condition_variable.h
index e8ab9e5..cd02ce4 100644
--- a/hal/include/HAL/cpp/priority_condition_variable.h
+++ b/hal/include/HAL/cpp/priority_condition_variable.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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. */
+/*----------------------------------------------------------------------------*/
+
#pragma once
/* std::condition_variable provides the native_handle() method to return its
diff --git a/hal/include/HAL/cpp/priority_mutex.h b/hal/include/HAL/cpp/priority_mutex.h
index 16e88f6..b03f012 100644
--- a/hal/include/HAL/cpp/priority_mutex.h
+++ b/hal/include/HAL/cpp/priority_mutex.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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. */
+/*----------------------------------------------------------------------------*/
+
#pragma once
// Allows usage with std::lock_guard without including <mutex> separately