diff --git a/.styleguide b/.styleguide index ab1c30a24b..6dbbb5c967 100644 --- a/.styleguide +++ b/.styleguide @@ -31,7 +31,6 @@ genFolderExclude { } genFileExclude { - CanTalonSRX\.h$ NIIMAQdx\.h$ can_proto\.h$ nivision\.h$ diff --git a/hal/include/HAL/CanTalonSRX.h b/hal/include/HAL/CanTalonSRX.h deleted file mode 100644 index 87e2bf020c..0000000000 --- a/hal/include/HAL/CanTalonSRX.h +++ /dev/null @@ -1,829 +0,0 @@ -/** - * @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 //CAN Comm -#include -#include -#include -#include -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 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 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 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 _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/lib/athena/ctre/CanTalonSRX.cpp b/hal/lib/athena/ctre/CanTalonSRX.cpp deleted file mode 100644 index 746506cbf8..0000000000 --- a/hal/lib/athena/ctre/CanTalonSRX.cpp +++ /dev/null @@ -1,2019 +0,0 @@ -/** - * @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 - */ -#include "HAL/CanTalonSRX.h" -#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm -#include // memset - -#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 STATUS_8 0x020415C0 -#define STATUS_9 0x02041600 - -#define CONTROL_1 0x02040000 -#define CONTROL_2 0x02040040 -#define CONTROL_3 0x02040080 -#define CONTROL_5 0x02040100 -#define CONTROL_6 0x02040140 - -#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) -#define GET_STATUS8() \ - CtreCanNode::recMsg rx = \ - GetRx(STATUS_8 | GetDeviceNumber(), \ - EXPECTED_RESPONSE_TIMEOUT_MS) -#define GET_STATUS9() \ - CtreCanNode::recMsg rx = \ - GetRx( \ - STATUS_9 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS) - -#define PARAM_REQUEST 0x02041800 -#define PARAM_RESPONSE 0x02041840 -#define PARAM_SET 0x02041880 - -const int kParamArbIdValue = PARAM_RESPONSE; -const int kParamArbIdMask = 0xFFFFFFFF; - -const double FLOAT_TO_FXP_10_22 = (double)0x400000; -const double FXP_TO_FLOAT_10_22 = 0.0000002384185791015625; - -const double FLOAT_TO_FXP_0_8 = (double)0x100; -const double FXP_TO_FLOAT_0_8 = 0.00390625; - -CanTalonSRX::CanTalonSRX(int deviceNumber, int controlPeriodMs, - int enablePeriodMs) - : CtreCanNode(deviceNumber), _can_h(0), _can_stat(0) { - _controlPeriodMs = controlPeriodMs; - _enablePeriodMs = enablePeriodMs; - - /* bound period to be within [1 ms,95 ms] */ - if (_controlPeriodMs < 1) - _controlPeriodMs = 1; - else if (_controlPeriodMs > 95) - _controlPeriodMs = 95; - if (_enablePeriodMs < 1) - _enablePeriodMs = 1; - else if (_enablePeriodMs > 95) - _enablePeriodMs = 95; - - RegisterRx(STATUS_1 | (UINT8)deviceNumber); - RegisterRx(STATUS_2 | (UINT8)deviceNumber); - RegisterRx(STATUS_3 | (UINT8)deviceNumber); - RegisterRx(STATUS_4 | (UINT8)deviceNumber); - RegisterRx(STATUS_5 | (UINT8)deviceNumber); - RegisterRx(STATUS_6 | (UINT8)deviceNumber); - RegisterRx(STATUS_7 | (UINT8)deviceNumber); - /* use the legacy command frame until we have evidence we can use the new - * frame. - */ - RegisterTx(CONTROL_1 | (UINT8)deviceNumber, (UINT8)_controlPeriodMs); - _controlFrameArbId = CONTROL_1; - /* the only default param that is nonzero is limit switch. - * Default to using the flash settings. - */ - SetOverrideLimitSwitchEn(kLimitSwitchOverride_UseDefaultsFromFlash); - /* Check if we can upgrade the control framing */ - UpdateControlId(); -} -/* CanTalonSRX D'tor - */ -CanTalonSRX::~CanTalonSRX() { - if (m_hasBeenMoved) { - /* Another CANTalonSRX still exists, so don't un-register the periodic - * control frame - */ - } else { - /* un-register the control frame so Talon is disabled */ - RegisterTx(CONTROL_1 | (UINT8)GetDeviceNumber(), 0); - RegisterTx(CONTROL_5 | (UINT8)GetDeviceNumber(), 0); - } - /* free the stream we used for SetParam/GetParamResponse */ - if (_can_h) { - FRC_NetworkCommunication_CANSessionMux_closeStreamSession(_can_h); - _can_h = 0; - } -} -/** - * @return true if Talon is reporting that it supports control5, and therefore - * RIO can send control5 to update control params (even when disabled). - */ -bool CanTalonSRX::IsControl5Supported() { - /* only bother to poll status2 if we are looking for cmd5allowed */ - GET_STATUS2(); - if (rx.err != CTR_OKAY) { - /* haven't received it */ - return false; - } else if (0 == rx->Cmd5Allowed) { - /* firmware doesn't support it */ - return false; - } - /* we can use control5, this gives application the ability to set control - * params prior to Talon-enable */ - return true; -} -/** - * Get a copy of the control frame to send. - * @param [out] pointer to eight byte array to fill. - */ -void CanTalonSRX::GetControlFrameCopy(uint8_t *toFill) { - /* get the copy of the control frame in control1 */ - CtreCanNode::txTask task = - GetTx(_controlFrameArbId | - GetDeviceNumber()); - /* control1's payload will move to 5, but update the new sigs in control5 */ - if (task.IsEmpty()) - memset(toFill, 0, 8); - else - memcpy(toFill, task.toSend, 8); - /* zero first two bytes - these are reserved. */ - toFill[0] = 0; - toFill[1] = 0; -} -/** - * 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 CanTalonSRX::UpdateControlId() { - uint8_t work[8]; - uint32_t frameToUse; - /* deduce if we should change IDs. If firm supports the new frame, and - * calling app isn't forcing legacy mode - * use control5.*/ - if (_useControl5ifSupported && IsControl5Supported()) { - frameToUse = CONTROL_5; - } else { - frameToUse = CONTROL_1; - } - /* is there anything to do */ - if (frameToUse == _controlFrameArbId) { - /* nothing to do, we are using the best frame. */ - } else if (frameToUse == CONTROL_5) { - /* get a copy of the control frame */ - GetControlFrameCopy(work); - /* Change control1's DLC to 2. Passing nullptr means all payload bytes are - * zero. */ - RegisterTx(CONTROL_1 | GetDeviceNumber(), _enablePeriodMs, 2, nullptr); - /* reregister the control frame using the new ID */ - RegisterTx(frameToUse | GetDeviceNumber(), _controlPeriodMs, 8, work); - /* save the correct frame ArbID */ - _controlFrameArbId = frameToUse; - } else if (frameToUse == CONTROL_1) { - GetControlFrameCopy(work); - /* stop sending control 5 */ - UnregisterTx(CONTROL_5 | GetDeviceNumber()); - /* reregister the control frame using the new ID */ - RegisterTx(frameToUse | GetDeviceNumber(), _controlPeriodMs, 8, work); - /* save the correct frame ArbID */ - _controlFrameArbId = frameToUse; - } -} -void CanTalonSRX::OpenSessionIfNeedBe() { - _can_stat = 0; - if (_can_h == 0) { - /* bit30 - bit8 must match $000002XX. Top bit is not masked to get remote - * frames */ - FRC_NetworkCommunication_CANSessionMux_openStreamSession( - &_can_h, kParamArbIdValue | GetDeviceNumber(), kParamArbIdMask, - kMsgCapacity, &_can_stat); - if (_can_stat == 0) { - /* success */ - } else { - /* something went wrong, try again later */ - _can_h = 0; - } - } -} -void CanTalonSRX::ProcessStreamMessages() { - if (0 == _can_h) OpenSessionIfNeedBe(); - /* process receive messages */ - uint32_t i; - uint32_t messagesToRead = sizeof(_msgBuff) / sizeof(_msgBuff[0]); - uint32_t messagesRead = 0; - /* read out latest bunch of messages */ - _can_stat = 0; - if (_can_h) { - FRC_NetworkCommunication_CANSessionMux_readStreamSession( - _can_h, _msgBuff, messagesToRead, &messagesRead, &_can_stat); - } - /* loop thru each message of interest */ - for (i = 0; i < messagesRead; ++i) { - tCANStreamMessage *msg = _msgBuff + i; - if (msg->messageID == (PARAM_RESPONSE | GetDeviceNumber())) { - TALON_Param_Response_t *paramResp = (TALON_Param_Response_t *)msg->data; - /* decode value */ - int32_t val = paramResp->ParamValueH; - val <<= 8; - val |= paramResp->ParamValueMH; - val <<= 8; - val |= paramResp->ParamValueML; - val <<= 8; - val |= paramResp->ParamValueL; - /* save latest signal */ - _sigs[paramResp->ParamEnum] = val; - } else { - int brkpthere = 42; - ++brkpthere; - } - } -} -void CanTalonSRX::Set(double value) { - if (value > 1) - value = 1; - else if (value < -1) - value = -1; - SetDemand(1023 * value); /* must be within [-1023,1023] */ -} -/*---------------------setters and getters that use the 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 CanTalonSRX::SetParamRaw(unsigned paramEnum, int rawBits) { - /* caller is using param API. Open session if it hasn'T been done. */ - if (0 == _can_h) OpenSessionIfNeedBe(); - TALON_Param_Response_t frame; - memset(&frame, 0, sizeof(frame)); - frame.ParamEnum = paramEnum; - frame.ParamValueH = rawBits >> 0x18; - frame.ParamValueMH = rawBits >> 0x10; - frame.ParamValueML = rawBits >> 0x08; - frame.ParamValueL = rawBits; - int32_t status = 0; - FRC_NetworkCommunication_CANSessionMux_sendMessage( - PARAM_SET | GetDeviceNumber(), (const uint8_t *)&frame, 5, 0, &status); - /* small hook here if we want the API itself to react to set commands */ - switch (paramEnum) { - case eLegacyControlMode: - if (rawBits != 0) { - /* caller wants to force legacy framing */ - _useControl5ifSupported = false; - } else { - /* caller wants to let the API decide */ - _useControl5ifSupported = true; - } - /* recheck IDs now that flag has changed */ - UpdateControlId(); - break; - } - /* for now have a general failure if we can't transmit */ - if (status) return CTR_TxFailed; - return CTR_OKAY; -} -/** - * Checks cached CAN frames and updating solicited signals. - */ -CTR_Code CanTalonSRX::GetParamResponseRaw(unsigned paramEnum, int &rawBits) { - CTR_Code retval = CTR_OKAY; - /* process received param events. We don't expect many since this API is not - * used often. */ - ProcessStreamMessages(); - /* grab the solicited signal value */ - sigs_t::iterator i = _sigs.find(paramEnum); - if (i == _sigs.end()) { - retval = CTR_SigNotUpdated; - } else { - rawBits = i->second; - } - return retval; -} -/** - * 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 CanTalonSRX::RequestParam(param_t paramEnum) { - /* process received param events. We don't expect many since this API is not - * used often. */ - ProcessStreamMessages(); - TALON_Param_Request_t frame; - memset(&frame, 0, sizeof(frame)); - frame.ParamEnum = paramEnum; - int32_t status = 0; - FRC_NetworkCommunication_CANSessionMux_sendMessage( - PARAM_REQUEST | GetDeviceNumber(), (const uint8_t *)&frame, 1, 0, - &status); - if (status) return CTR_TxFailed; - return CTR_OKAY; -} - -CTR_Code CanTalonSRX::SetParam(param_t paramEnum, double value) { - int32_t rawbits = 0; - switch (paramEnum) { - case eProfileParamSlot0_P: /* unsigned 10.22 fixed pt value */ - case eProfileParamSlot0_I: - case eProfileParamSlot0_D: - case eProfileParamSlot1_P: - case eProfileParamSlot1_I: - case eProfileParamSlot1_D: { - uint32_t urawbits; - value = std::min( - value, 1023.0); /* bounds check doubles that are outside u10.22 */ - value = std::max(value, 0.0); - urawbits = value * FLOAT_TO_FXP_10_22; /* perform unsign arithmetic */ - rawbits = urawbits; /* copy bits over. SetParamRaw just stuffs into CAN - frame with no sense of signedness */ - } break; - case eProfileParamSlot1_F: /* signed 10.22 fixed pt value */ - case eProfileParamSlot0_F: - value = std::min( - value, 512.0); /* bounds check doubles that are outside s10.22 */ - value = std::max(value, -512.0); - rawbits = value * FLOAT_TO_FXP_10_22; - break; - case eProfileParamVcompRate: /* unsigned 0.8 fixed pt value volts per ms */ - /* within [0,1) volts per ms. - Slowest ramp is 1/256 VperMilliSec or 3.072 seconds from 0-to-12V. - Fastest ramp is 255/256 VperMilliSec or 12.1ms from 0-to-12V. - */ - if (value <= 0) { - /* negative or zero (disable), send raw value of zero */ - rawbits = 0; - } else { - /* nonzero ramping */ - rawbits = value * FLOAT_TO_FXP_0_8; - /* since whole part is cleared, cap to just under whole unit */ - if (rawbits > (FLOAT_TO_FXP_0_8 - 1)) rawbits = (FLOAT_TO_FXP_0_8 - 1); - /* since ramping is nonzero, cap to smallest ramp rate possible */ - if (rawbits == 0) { - /* caller is providing a nonzero ramp rate that's too small - to serialize, so cap to smallest possible */ - rawbits = 1; - } - } - break; - default: /* everything else is integral */ - rawbits = (int32_t)value; - break; - } - return SetParamRaw(paramEnum, rawbits); -} -CTR_Code CanTalonSRX::GetParamResponse(param_t paramEnum, double &value) { - int32_t rawbits = 0; - CTR_Code retval = GetParamResponseRaw(paramEnum, rawbits); - switch (paramEnum) { - case eProfileParamSlot0_P: /* 10.22 fixed pt value */ - case eProfileParamSlot0_I: - case eProfileParamSlot0_D: - case eProfileParamSlot0_F: - case eProfileParamSlot1_P: - case eProfileParamSlot1_I: - case eProfileParamSlot1_D: - case eProfileParamSlot1_F: - case eCurrent: - case eTemp: - case eBatteryV: - value = ((double)rawbits) * FXP_TO_FLOAT_10_22; - break; - case eProfileParamVcompRate: - value = ((double)rawbits) * FXP_TO_FLOAT_0_8; - break; - default: /* everything else is integral */ - value = (double)rawbits; - break; - } - return retval; -} -CTR_Code CanTalonSRX::GetParamResponseInt32(param_t paramEnum, int &value) { - double dvalue = 0; - CTR_Code retval = GetParamResponse(paramEnum, dvalue); - value = (int32_t)dvalue; - return retval; -} -/*----- 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 CanTalonSRX::SetPgain(unsigned slotIdx, double gain) { - if (slotIdx == 0) return SetParam(eProfileParamSlot0_P, gain); - return SetParam(eProfileParamSlot1_P, gain); -} -CTR_Code CanTalonSRX::SetIgain(unsigned slotIdx, double gain) { - if (slotIdx == 0) return SetParam(eProfileParamSlot0_I, gain); - return SetParam(eProfileParamSlot1_I, gain); -} -CTR_Code CanTalonSRX::SetDgain(unsigned slotIdx, double gain) { - if (slotIdx == 0) return SetParam(eProfileParamSlot0_D, gain); - return SetParam(eProfileParamSlot1_D, gain); -} -CTR_Code CanTalonSRX::SetFgain(unsigned slotIdx, double gain) { - if (slotIdx == 0) return SetParam(eProfileParamSlot0_F, gain); - return SetParam(eProfileParamSlot1_F, gain); -} -CTR_Code CanTalonSRX::SetIzone(unsigned slotIdx, int zone) { - if (slotIdx == 0) return SetParam(eProfileParamSlot0_IZone, zone); - return SetParam(eProfileParamSlot1_IZone, zone); -} -CTR_Code CanTalonSRX::SetCloseLoopRampRate(unsigned slotIdx, - int closeLoopRampRate) { - if (slotIdx == 0) - return SetParam(eProfileParamSlot0_CloseLoopRampRate, closeLoopRampRate); - return SetParam(eProfileParamSlot1_CloseLoopRampRate, closeLoopRampRate); -} -CTR_Code CanTalonSRX::SetVoltageCompensationRate(double voltagePerMs) { - return SetParam(eProfileParamVcompRate, voltagePerMs); -} -CTR_Code CanTalonSRX::GetPgain(unsigned slotIdx, double &gain) { - if (slotIdx == 0) return GetParamResponse(eProfileParamSlot0_P, gain); - return GetParamResponse(eProfileParamSlot1_P, gain); -} -CTR_Code CanTalonSRX::GetIgain(unsigned slotIdx, double &gain) { - if (slotIdx == 0) return GetParamResponse(eProfileParamSlot0_I, gain); - return GetParamResponse(eProfileParamSlot1_I, gain); -} -CTR_Code CanTalonSRX::GetDgain(unsigned slotIdx, double &gain) { - if (slotIdx == 0) return GetParamResponse(eProfileParamSlot0_D, gain); - return GetParamResponse(eProfileParamSlot1_D, gain); -} -CTR_Code CanTalonSRX::GetFgain(unsigned slotIdx, double &gain) { - if (slotIdx == 0) return GetParamResponse(eProfileParamSlot0_F, gain); - return GetParamResponse(eProfileParamSlot1_F, gain); -} -CTR_Code CanTalonSRX::GetIzone(unsigned slotIdx, int &zone) { - if (slotIdx == 0) - return GetParamResponseInt32(eProfileParamSlot0_IZone, zone); - return GetParamResponseInt32(eProfileParamSlot1_IZone, zone); -} -CTR_Code CanTalonSRX::GetCloseLoopRampRate(unsigned slotIdx, - int &closeLoopRampRate) { - if (slotIdx == 0) - return GetParamResponseInt32(eProfileParamSlot0_CloseLoopRampRate, - closeLoopRampRate); - return GetParamResponseInt32(eProfileParamSlot1_CloseLoopRampRate, - closeLoopRampRate); -} -CTR_Code CanTalonSRX::GetVoltageCompensationRate(double &voltagePerMs) { - return GetParamResponse(eProfileParamVcompRate, voltagePerMs); -} -CTR_Code CanTalonSRX::SetSensorPosition(int pos) { - return SetParam(eSensorPosition, pos); -} -CTR_Code CanTalonSRX::SetForwardSoftLimit(int forwardLimit) { - return SetParam(eProfileParamSoftLimitForThreshold, forwardLimit); -} -CTR_Code CanTalonSRX::SetReverseSoftLimit(int reverseLimit) { - return SetParam(eProfileParamSoftLimitRevThreshold, reverseLimit); -} -CTR_Code CanTalonSRX::SetForwardSoftEnable(int enable) { - return SetParam(eProfileParamSoftLimitForEnable, enable); -} -CTR_Code CanTalonSRX::SetReverseSoftEnable(int enable) { - return SetParam(eProfileParamSoftLimitRevEnable, enable); -} -CTR_Code CanTalonSRX::GetForwardSoftLimit(int &forwardLimit) { - return GetParamResponseInt32(eProfileParamSoftLimitForThreshold, - forwardLimit); -} -CTR_Code CanTalonSRX::GetReverseSoftLimit(int &reverseLimit) { - return GetParamResponseInt32(eProfileParamSoftLimitRevThreshold, - reverseLimit); -} -CTR_Code CanTalonSRX::GetForwardSoftEnable(int &enable) { - return GetParamResponseInt32(eProfileParamSoftLimitForEnable, enable); -} -CTR_Code CanTalonSRX::GetReverseSoftEnable(int &enable) { - return GetParamResponseInt32(eProfileParamSoftLimitRevEnable, enable); -} -/** - * @param param [out] Rise to fall time period in microseconds. - */ -CTR_Code CanTalonSRX::GetPulseWidthRiseToFallUs(int ¶m) { - int temp = 0; - int periodUs = 0; - /* first grab our 12.12 position */ - CTR_Code retval1 = GetPulseWidthPosition(temp); - /* mask off number of turns */ - temp &= 0xFFF; - /* next grab the waveform period. This value - * will be zero if we stop getting pulses **/ - CTR_Code retval2 = GetPulseWidthRiseToRiseUs(periodUs); - /* now we have 0.12 position that is scaled to the waveform period. - Use fixed pt multiply to scale our 0.16 period into us.*/ - param = (temp * periodUs) / BIT12; - /* pass the worst error code to caller. - Assume largest value is the most pressing error code.*/ - return (CTR_Code)std::max((int)retval1, (int)retval2); -} -CTR_Code CanTalonSRX::IsPulseWidthSensorPresent(int ¶m) { - int periodUs = 0; - CTR_Code retval = GetPulseWidthRiseToRiseUs(periodUs); - /* if a nonzero period is present, we are getting good pules. - Otherwise the sensor is not present. */ - if (periodUs != 0) - param = 1; - else - param = 0; - return retval; -} -/** - * @param modeSelect selects which mode. - * @param demand setpt or throttle or masterId to follow. - * @return error code, 0 iff successful. - * This function has the advantage of atomically setting mode and demand. - */ -CTR_Code CanTalonSRX::SetModeSelect(int modeSelect, int demand) { - CtreCanNode::txTask toFill = - GetTx(_controlFrameArbId | - GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->ModeSelect = modeSelect; - toFill->DemandH = demand >> 16; - toFill->DemandM = demand >> 8; - toFill->DemandL = demand >> 0; - FlushTx(toFill); - return CTR_OKAY; -} -/** - * Change the periodMs of a TALON's status frame. See kStatusFrame_* enums for - * what's available. - */ -CTR_Code CanTalonSRX::SetStatusFrameRate(unsigned frameEnum, - unsigned periodMs) { - CTR_Code retval = CTR_OKAY; - int32_t paramEnum = 0; - /* bounds check the period */ - if (periodMs < 1) - periodMs = 1; - else if (periodMs > 255) - periodMs = 255; - uint8_t period = (uint8_t)periodMs; - /* lookup the correct param enum based on what frame to rate-change */ - switch (frameEnum) { - case kStatusFrame_General: - paramEnum = eStatus1FrameRate; - break; - case kStatusFrame_Feedback: - paramEnum = eStatus2FrameRate; - break; - case kStatusFrame_Encoder: - paramEnum = eStatus3FrameRate; - break; - case kStatusFrame_AnalogTempVbat: - paramEnum = eStatus4FrameRate; - break; - case kStatusFrame_PulseWidthMeas: - paramEnum = eStatus8FrameRate; - break; - case kStatusFrame_MotionProfile: - paramEnum = eStatus9FrameRate; - break; - default: - /* caller's request is not support, return an error code */ - retval = CTR_InvalidParamValue; - break; - } - /* if lookup was succesful, send set-request out */ - if (retval == CTR_OKAY) { - /* paramEnum is updated, sent it out */ - retval = SetParamRaw(paramEnum, period); - } - return retval; -} -/** - * Clear all sticky faults in TALON. - */ -CTR_Code CanTalonSRX::ClearStickyFaults() { - int32_t status = 0; - /* build request frame */ - TALON_Control_3_ClearFlags_OneShot_t frame; - memset(&frame, 0, sizeof(frame)); - frame.ClearStickyFaults = 1; - FRC_NetworkCommunication_CANSessionMux_sendMessage( - CONTROL_3 | GetDeviceNumber(), (const uint8_t *)&frame, sizeof(frame), 0, - &status); - if (status) return CTR_TxFailed; - return CTR_OKAY; -} -/** - * @return the tx task that transmits Control6 (motion profile control). - * If it's not scheduled, then schedule it. This is part of firing - * the MotionProf framing only when needed to save bandwidth. - */ -CtreCanNode::txTask -CanTalonSRX::GetControl6() { - CtreCanNode::txTask control6 = - GetTx(CONTROL_6 | - GetDeviceNumber()); - if (control6.IsEmpty()) { - /* control6 never started, arm it now */ - RegisterTx(CONTROL_6 | GetDeviceNumber(), _control6PeriodMs); - control6 = GetTx(CONTROL_6 | - GetDeviceNumber()); - control6->Idx = 0; - _motProfFlowControl = 0; - FlushTx(control6); - } - return control6; -} -/** - * 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 CanTalonSRX::ChangeMotionControlFramePeriod(uint32_t periodMs) { - std::unique_lock lock(_mutMotProf); - /* if message is already registered, it will get updated. - * Otherwise it will error if it hasn't been setup yet, but that's ok - * because the _control6PeriodMs will be used later. - * @see GetControl6 - */ - _control6PeriodMs = periodMs; - ChangeTxPeriod(CONTROL_6 | GetDeviceNumber(), _control6PeriodMs); -} -/** - * Clear the buffered motion profile in both Talon RAM (bottom), and in the API - * (top). - */ -void CanTalonSRX::ClearMotionProfileTrajectories() { - std::unique_lock lock(_mutMotProf); - /* clear the top buffer */ - _motProfTopBuffer.Clear(); - /* send signal to clear bottom buffer */ - auto toFill = CanTalonSRX::GetControl6(); - toFill->Idx = 0; - _motProfFlowControl = 0; /* match the transmitted flow control */ - FlushTx(toFill); -} -/** - * 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 CanTalonSRX::GetMotionProfileTopLevelBufferCount() { - std::unique_lock lock(_mutMotProf); - uint32_t retval = (uint32_t)_motProfTopBuffer.GetNumTrajectories(); - return retval; -} -/** - * 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 CanTalonSRX::IsMotionProfileTopLevelBufferFull() { - std::unique_lock lock(_mutMotProf); - if (_motProfTopBuffer.GetNumTrajectories() >= kMotionProfileTopBufferCapacity) - return true; - return false; -} -/** - * 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 - * never 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 CanTalonSRX::PushMotionProfileTrajectory(int targPos, int targVel, - int profileSlotSelect, - int timeDurMs, int velOnly, - int isLastPoint, - int zeroPos) { - ReactToMotionProfileCall(); - /* create our trajectory point */ - TALON_Control_6_MotProfAddTrajPoint_huff0_t traj; - memset((void *)&traj, 0, sizeof(traj)); - traj.NextPt_ZeroPosition = zeroPos ? 1 : 0; - traj.NextPt_VelOnly = velOnly ? 1 : 0; - traj.NextPt_IsLast = isLastPoint ? 1 : 0; - traj.NextPt_ProfileSlotSelect = (profileSlotSelect > 0) ? 1 : 0; - if (timeDurMs < 0) - timeDurMs = 0; - else if (timeDurMs > 255) - timeDurMs = 255; - traj.NextPt_DurationMs = timeDurMs; - traj.NextPt_VelocityH = targVel >> 0x08; - traj.NextPt_VelocityL = targVel & 0xFF; - traj.NextPt_PositionH = targPos >> 0x10; - traj.NextPt_PositionM = targPos >> 0x08; - traj.NextPt_PositionL = targPos & 0xFF; - - std::unique_lock lock(_mutMotProf); - if (_motProfTopBuffer.GetNumTrajectories() >= kMotionProfileTopBufferCapacity) - return CTR_BufferFull; - _motProfTopBuffer.Push(traj); - return CTR_OKAY; -} -/** - * Increment our flow control to manage streaming to the Talon. - * f(x) = { 1, x = 15, - * x+1, x < 15 - * } - */ -#define MotionProf_IncrementSync(idx) ((idx >= 15) ? 1 : 0) + ((idx + 1) & 0xF) -/** - * 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 CanTalonSRX::CopyTrajPtIntoControl( - TALON_Control_6_MotProfAddTrajPoint_t *control, - const TALON_Control_6_MotProfAddTrajPoint_t *newPt) { - /* Bring over the common signals in the first two bytes */ - control->NextPt_ProfileSlotSelect = newPt->NextPt_ProfileSlotSelect; - control->NextPt_ZeroPosition = newPt->NextPt_ZeroPosition; - control->NextPt_VelOnly = newPt->NextPt_VelOnly; - control->NextPt_IsLast = newPt->NextPt_IsLast; - control->huffCode = newPt->huffCode; - /* the last six bytes are entirely for hold NextPt's values. */ - uint8_t *dest = (uint8_t *)control; - const uint8_t *src = (const uint8_t *)newPt; - dest[2] = src[2]; - dest[3] = src[3]; - dest[4] = src[4]; - dest[5] = src[5]; - dest[6] = src[6]; - dest[7] = src[7]; -} -/** - * 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 CanTalonSRX::ReactToMotionProfileCall() { - if (_motProfFlowControl < 0) { - /* we have not yet armed the periodic frame. We do this lazilly to - * save bus utilization since most Talons on the bus probably are not - * MP'ing. - */ - ClearMotionProfileTrajectories(); /* this moves flow control so only fires - once if ever */ - } -} -/** - * 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 CanTalonSRX::ProcessMotionProfileBuffer() { - ReactToMotionProfileCall(); - /* get the latest status frame */ - GET_STATUS9(); - /* lock */ - std::unique_lock lock(_mutMotProf); - /* calc what we expect to receive */ - if (_motProfFlowControl == rx->NextID) { - /* Talon has completed the last req */ - if (_motProfTopBuffer.IsEmpty()) { - /* nothing to do */ - } else { - /* get the latest control frame */ - auto toFill = GetControl6(); - TALON_Control_6_MotProfAddTrajPoint_t *front = _motProfTopBuffer.Front(); - CopyTrajPtIntoControl(toFill.toSend, front); - _motProfTopBuffer.Pop(); - _motProfFlowControl = MotionProf_IncrementSync(_motProfFlowControl); - toFill->Idx = _motProfFlowControl; - FlushTx(toFill); - } - } else { - /* still waiting on Talon */ - } -} -/** - * 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 CanTalonSRX::GetMotionProfileStatus( - uint32_t &flags, uint32_t &profileSlotSelect, int32_t &targPos, - int32_t &targVel, uint32_t &topBufferRem, uint32_t &topBufferCnt, - uint32_t &btmBufferCnt, uint32_t &outputEnable) { - /* get the latest status frame */ - GET_STATUS9(); - - /* clear signals in case we never received an update, caller should check - * return - */ - flags = 0; - profileSlotSelect = 0; - targPos = 0; - targVel = 0; - btmBufferCnt = 0; - - /* these signals are always available */ - topBufferCnt = _motProfTopBuffer.GetNumTrajectories(); - topBufferRem = - kMotionProfileTopBufferCapacity - _motProfTopBuffer.GetNumTrajectories(); - - /* TODO: make enums or make a better method prototype */ - if (rx->ActTraj_IsValid) flags |= kMotionProfileFlag_ActTraj_IsValid; - if (rx->HasUnderrun) flags |= kMotionProfileFlag_HasUnderrun; - if (rx->IsUnderrun) flags |= kMotionProfileFlag_IsUnderrun; - if (rx->ActTraj_IsLast) flags |= kMotionProfileFlag_ActTraj_IsLast; - if (rx->ActTraj_VelOnly) flags |= kMotionProfileFlag_ActTraj_VelOnly; - - btmBufferCnt = rx->Count; - - targPos = rx->ActTraj_PositionH; - targPos <<= 8; - targPos |= rx->ActTraj_PositionM; - targPos <<= 8; - targPos |= rx->ActTraj_PositionL; - - targVel = rx->ActTraj_VelocityH; - targVel <<= 8; - targVel |= rx->ActTraj_VelocityL; - - profileSlotSelect = rx->ActTraj_ProfileSlotSelect; - - switch (rx->OutputType) { - case kMotionProf_Disabled: - case kMotionProf_Enable: - case kMotionProf_Hold: - outputEnable = rx->OutputType; - break; - default: - /* do now allow invalid values for sake of user-facing enum types */ - outputEnable = kMotionProf_Disabled; - break; - } - return rx.err; -} -//------------------------ 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 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::GetAppliedThrottle(int ¶m) -{ - GET_STATUS1(); - int32_t raw = 0; - raw |= rx->AppliedThrottle_h3; - raw <<= 8; - raw |= rx->AppliedThrottle_l8; - raw <<= (32-11); /* sign extend */ - raw >>= (32-11); /* sign extend */ - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetCloseLoopErr(int ¶m) -{ - GET_STATUS1(); - int32_t raw = 0; - raw |= rx->CloseLoopErrH; - raw <<= 16 - 8; - raw |= rx->CloseLoopErrM; - raw <<= 8; - raw |= rx->CloseLoopErrL; - raw <<= (32-24); /* sign extend */ - raw >>= (32-24); /* sign extend */ - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetFeedbackDeviceSelect(int ¶m) -{ - GET_STATUS1(); - param = rx->FeedbackDeviceSelect; - 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::GetSensorPosition(int ¶m) -{ - GET_STATUS2(); - int32_t raw = 0; - raw |= rx->SensorPositionH; - raw <<= 16 - 8; - raw |= rx->SensorPositionM; - raw <<= 8; - raw |= rx->SensorPositionL; - raw <<= (32-24); /* sign extend */ - raw >>= (32-24); /* sign extend */ - if(rx->PosDiv8) - raw *= 8; - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetSensorVelocity(int ¶m) -{ - GET_STATUS2(); - int32_t raw = 0; - raw |= rx->SensorVelocityH; - raw <<= 8; - raw |= rx->SensorVelocityL; - raw <<= (32-16); /* sign extend */ - raw >>= (32-16); /* sign extend */ - if(rx->VelDiv4) - raw *= 4; - 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(); - int32_t raw = 0; - raw |= rx->EncPositionH; - raw <<= 16 - 8; - raw |= rx->EncPositionM; - raw <<= 8; - raw |= rx->EncPositionL; - raw <<= (32-24); /* sign extend */ - raw >>= (32-24); /* sign extend */ - if(rx->PosDiv8) - raw *= 8; - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetEncVel(int ¶m) -{ - GET_STATUS3(); - int32_t raw = 0; - raw |= rx->EncVelH; - raw <<= 8; - raw |= rx->EncVelL; - raw <<= (32-16); /* sign extend */ - raw >>= (32-16); /* sign extend */ - if(rx->VelDiv4) - raw *= 4; - 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(); - int32_t raw = 0; - raw |= rx->AnalogInWithOvH; - raw <<= 16 - 8; - raw |= rx->AnalogInWithOvM; - raw <<= 8; - raw |= rx->AnalogInWithOvL; - raw <<= (32-24); /* sign extend */ - raw >>= (32-24); /* sign extend */ - if(rx->PosDiv8) - raw *= 8; - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetAnalogInVel(int ¶m) -{ - GET_STATUS4(); - int32_t raw = 0; - raw |= rx->AnalogInVelH; - raw <<= 8; - raw |= rx->AnalogInVelL; - raw <<= (32-16); /* sign extend */ - raw >>= (32-16); /* sign extend */ - if(rx->VelDiv4) - raw *= 4; - 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::GetPulseWidthPosition(int ¶m) -{ - GET_STATUS8(); - int32_t raw = 0; - raw |= rx->PulseWidPositionH; - raw <<= 16 - 8; - raw |= rx->PulseWidPositionM; - raw <<= 8; - raw |= rx->PulseWidPositionL; - raw <<= (32-24); /* sign extend */ - raw >>= (32-24); /* sign extend */ - if(rx->PosDiv8) - raw *= 8; - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetPulseWidthVelocity(int ¶m) -{ - GET_STATUS8(); - int32_t raw = 0; - raw |= rx->PulseWidVelH; - raw <<= 8; - raw |= rx->PulseWidVelL; - raw <<= (32-16); /* sign extend */ - raw >>= (32-16); /* sign extend */ - if(rx->VelDiv4) - raw *= 4; - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetPulseWidthRiseToRiseUs(int ¶m) -{ - GET_STATUS8(); - uint32_t raw = 0; - raw |= rx->PeriodUsM8; - raw <<= 8; - raw |= rx->PeriodUsL8; - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetActTraj_IsValid(int ¶m) -{ - GET_STATUS9(); - param = rx->ActTraj_IsValid; - return rx.err; -} -CTR_Code CanTalonSRX::GetActTraj_ProfileSlotSelect(int ¶m) -{ - GET_STATUS9(); - param = rx->ActTraj_ProfileSlotSelect; - return rx.err; -} -CTR_Code CanTalonSRX::GetActTraj_VelOnly(int ¶m) -{ - GET_STATUS9(); - param = rx->ActTraj_VelOnly; - return rx.err; -} -CTR_Code CanTalonSRX::GetActTraj_IsLast(int ¶m) -{ - GET_STATUS9(); - param = rx->ActTraj_IsLast; - return rx.err; -} -CTR_Code CanTalonSRX::GetOutputType(int ¶m) -{ - GET_STATUS9(); - param = rx->OutputType; - return rx.err; -} -CTR_Code CanTalonSRX::GetHasUnderrun(int ¶m) -{ - GET_STATUS9(); - param = rx->HasUnderrun; - return rx.err; -} -CTR_Code CanTalonSRX::GetIsUnderrun(int ¶m) -{ - GET_STATUS9(); - param = rx->IsUnderrun; - return rx.err; -} -CTR_Code CanTalonSRX::GetNextID(int ¶m) -{ - GET_STATUS9(); - param = rx->NextID; - return rx.err; -} -CTR_Code CanTalonSRX::GetBufferIsFull(int ¶m) -{ - GET_STATUS9(); - param = rx->BufferIsFull; - return rx.err; -} -CTR_Code CanTalonSRX::GetCount(int ¶m) -{ - GET_STATUS9(); - param = rx->Count; - return rx.err; -} -CTR_Code CanTalonSRX::GetActTraj_Velocity(int ¶m) -{ - GET_STATUS9(); - int32_t raw = 0; - raw |= rx->ActTraj_VelocityH; - raw <<= 8; - raw |= rx->ActTraj_VelocityL; - raw <<= (32-16); /* sign extend */ - raw >>= (32-16); /* sign extend */ - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::GetActTraj_Position(int ¶m) -{ - GET_STATUS9(); - int32_t raw = 0; - raw |= rx->ActTraj_PositionH; - raw <<= 16 - 8; - raw |= rx->ActTraj_PositionM; - raw <<= 8; - raw |= rx->ActTraj_PositionL; - raw <<= (32-24); /* sign extend */ - raw >>= (32-24); /* sign extend */ - param = (int)raw; - return rx.err; -} -CTR_Code CanTalonSRX::SetDemand(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->DemandH = param>>16; - toFill->DemandM = param>>8; - toFill->DemandL = param>>0; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetOverrideLimitSwitchEn(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->OverrideLimitSwitchEn = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetFeedbackDeviceSelect(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->FeedbackDeviceSelect = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetRevMotDuringCloseLoopEn(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->RevMotDuringCloseLoopEn = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetOverrideBrakeType(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->OverrideBrakeType = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetModeSelect(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->ModeSelect = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetProfileSlotSelect(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->ProfileSlotSelect = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetRampThrottle(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->RampThrottle = param; - FlushTx(toFill); - return CTR_OKAY; -} -CTR_Code CanTalonSRX::SetRevFeedbackSensor(int param) -{ - CtreCanNode::txTask toFill = GetTx(_controlFrameArbId | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->RevFeedbackSensor = param ? 1 : 0; - FlushTx(toFill); - return CTR_OKAY; -} -//------------------ C interface --------------------------------------------// -extern "C" { -void *c_TalonSRX_Create3(int deviceNumber, int controlPeriodMs, int enablePeriodMs) -{ - return new CanTalonSRX(deviceNumber, controlPeriodMs, enablePeriodMs); -} -void *c_TalonSRX_Create2(int deviceNumber, int controlPeriodMs) -{ - return new CanTalonSRX(deviceNumber, controlPeriodMs); -} -void *c_TalonSRX_Create1(int deviceNumber) -{ - return new CanTalonSRX(deviceNumber); -} -void c_TalonSRX_Destroy(void *handle) -{ - delete (CanTalonSRX*)handle; -} -void c_TalonSRX_Set(void *handle, double value) -{ - return ((CanTalonSRX*)handle)->Set(value); -} -CTR_Code c_TalonSRX_SetParam(void *handle, int paramEnum, double value) -{ - return ((CanTalonSRX*)handle)->SetParam((CanTalonSRX::param_t)paramEnum, value); -} -CTR_Code c_TalonSRX_RequestParam(void *handle, int paramEnum) -{ - return ((CanTalonSRX*)handle)->RequestParam((CanTalonSRX::param_t)paramEnum); -} -CTR_Code c_TalonSRX_GetParamResponse(void *handle, int paramEnum, double *value) -{ - return ((CanTalonSRX*)handle)->GetParamResponse((CanTalonSRX::param_t)paramEnum, *value); -} -CTR_Code c_TalonSRX_GetParamResponseInt32(void *handle, int paramEnum, int *value) -{ - return ((CanTalonSRX*)handle)->GetParamResponseInt32((CanTalonSRX::param_t)paramEnum, *value); -} -CTR_Code c_TalonSRX_SetPgain(void *handle, int slotIdx, double gain) -{ - return ((CanTalonSRX*)handle)->SetPgain((unsigned)slotIdx, gain); -} -CTR_Code c_TalonSRX_SetIgain(void *handle, int slotIdx, double gain) -{ - return ((CanTalonSRX*)handle)->SetIgain((unsigned)slotIdx, gain); -} -CTR_Code c_TalonSRX_SetDgain(void *handle, int slotIdx, double gain) -{ - return ((CanTalonSRX*)handle)->SetDgain((unsigned)slotIdx, gain); -} -CTR_Code c_TalonSRX_SetFgain(void *handle, int slotIdx, double gain) -{ - return ((CanTalonSRX*)handle)->SetFgain((unsigned)slotIdx, gain); -} -CTR_Code c_TalonSRX_SetIzone(void *handle, int slotIdx, int zone) -{ - return ((CanTalonSRX*)handle)->SetIzone((unsigned)slotIdx, zone); -} -CTR_Code c_TalonSRX_SetCloseLoopRampRate(void *handle, int slotIdx, int closeLoopRampRate) -{ - return ((CanTalonSRX*)handle)->SetCloseLoopRampRate((unsigned)slotIdx, closeLoopRampRate); -} -CTR_Code c_TalonSRX_SetVoltageCompensationRate(void *handle, double voltagePerMs) -{ - return ((CanTalonSRX*)handle)->SetVoltageCompensationRate(voltagePerMs); -} -CTR_Code c_TalonSRX_SetSensorPosition(void *handle, int pos) -{ - return ((CanTalonSRX*)handle)->SetSensorPosition(pos); -} -CTR_Code c_TalonSRX_SetForwardSoftLimit(void *handle, int forwardLimit) -{ - return ((CanTalonSRX*)handle)->SetForwardSoftLimit(forwardLimit); -} -CTR_Code c_TalonSRX_SetReverseSoftLimit(void *handle, int reverseLimit) -{ - return ((CanTalonSRX*)handle)->SetReverseSoftLimit(reverseLimit); -} -CTR_Code c_TalonSRX_SetForwardSoftEnable(void *handle, int enable) -{ - return ((CanTalonSRX*)handle)->SetForwardSoftEnable(enable); -} -CTR_Code c_TalonSRX_SetReverseSoftEnable(void *handle, int enable) -{ - return ((CanTalonSRX*)handle)->SetReverseSoftEnable(enable); -} -CTR_Code c_TalonSRX_GetPgain(void *handle, int slotIdx, double *gain) -{ - return ((CanTalonSRX*)handle)->GetPgain((unsigned)slotIdx, *gain); -} -CTR_Code c_TalonSRX_GetIgain(void *handle, int slotIdx, double *gain) -{ - return ((CanTalonSRX*)handle)->GetIgain((unsigned)slotIdx, *gain); -} -CTR_Code c_TalonSRX_GetDgain(void *handle, int slotIdx, double *gain) -{ - return ((CanTalonSRX*)handle)->GetDgain((unsigned)slotIdx, *gain); -} -CTR_Code c_TalonSRX_GetFgain(void *handle, int slotIdx, double *gain) -{ - return ((CanTalonSRX*)handle)->GetFgain((unsigned)slotIdx, *gain); -} -CTR_Code c_TalonSRX_GetIzone(void *handle, int slotIdx, int *zone) -{ - return ((CanTalonSRX*)handle)->GetIzone((unsigned)slotIdx, *zone); -} -CTR_Code c_TalonSRX_GetCloseLoopRampRate(void *handle, int slotIdx, int *closeLoopRampRate) -{ - return ((CanTalonSRX*)handle)->GetCloseLoopRampRate((unsigned)slotIdx, *closeLoopRampRate); -} -CTR_Code c_TalonSRX_GetVoltageCompensationRate(void *handle, double *voltagePerMs) -{ - return ((CanTalonSRX*)handle)->GetVoltageCompensationRate(*voltagePerMs); -} -CTR_Code c_TalonSRX_GetForwardSoftLimit(void *handle, int *forwardLimit) -{ - return ((CanTalonSRX*)handle)->GetForwardSoftLimit(*forwardLimit); -} -CTR_Code c_TalonSRX_GetReverseSoftLimit(void *handle, int *reverseLimit) -{ - return ((CanTalonSRX*)handle)->GetReverseSoftLimit(*reverseLimit); -} -CTR_Code c_TalonSRX_GetForwardSoftEnable(void *handle, int *enable) -{ - return ((CanTalonSRX*)handle)->GetForwardSoftEnable(*enable); -} -CTR_Code c_TalonSRX_GetReverseSoftEnable(void *handle, int *enable) -{ - return ((CanTalonSRX*)handle)->GetReverseSoftEnable(*enable); -} -CTR_Code c_TalonSRX_GetPulseWidthRiseToFallUs(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetPulseWidthRiseToFallUs(*param); -} -CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->IsPulseWidthSensorPresent(*param); -} -CTR_Code c_TalonSRX_SetModeSelect2(void *handle, int modeSelect, int demand) -{ - return ((CanTalonSRX*)handle)->SetModeSelect(modeSelect, demand); -} -CTR_Code c_TalonSRX_SetStatusFrameRate(void *handle, int frameEnum, int periodMs) -{ - return ((CanTalonSRX*)handle)->SetStatusFrameRate((unsigned)frameEnum, (unsigned)periodMs); -} -CTR_Code c_TalonSRX_ClearStickyFaults(void *handle) -{ - return ((CanTalonSRX*)handle)->ClearStickyFaults(); -} -void c_TalonSRX_ChangeMotionControlFramePeriod(void *handle, int periodMs) -{ - return ((CanTalonSRX*)handle)->ChangeMotionControlFramePeriod((uint32_t)periodMs); -} -void c_TalonSRX_ClearMotionProfileTrajectories(void *handle) -{ - return ((CanTalonSRX*)handle)->ClearMotionProfileTrajectories(); -} -int c_TalonSRX_GetMotionProfileTopLevelBufferCount(void *handle) -{ - return ((CanTalonSRX*)handle)->GetMotionProfileTopLevelBufferCount(); -} -int c_TalonSRX_IsMotionProfileTopLevelBufferFull(void *handle) -{ - return ((CanTalonSRX*)handle)->IsMotionProfileTopLevelBufferFull(); -} -CTR_Code c_TalonSRX_PushMotionProfileTrajectory(void *handle, int targPos, int targVel, int profileSlotSelect, int timeDurMs, int velOnly, int isLastPoint, int zeroPos) -{ - return ((CanTalonSRX*)handle)->PushMotionProfileTrajectory(targPos, targVel, profileSlotSelect, timeDurMs, velOnly, isLastPoint, zeroPos); -} -void c_TalonSRX_ProcessMotionProfileBuffer(void *handle) -{ - return ((CanTalonSRX*)handle)->ProcessMotionProfileBuffer(); -} -CTR_Code c_TalonSRX_GetMotionProfileStatus(void *handle, int *flags, int *profileSlotSelect, int *targPos, int *targVel, int *topBufferRemaining, int *topBufferCnt, int *btmBufferCnt, int *outputEnable) -{ - uint32_t flags_val; - uint32_t profileSlotSelect_val; - int32_t targPos_val; - int32_t targVel_val; - uint32_t topBufferRemaining_val; - uint32_t topBufferCnt_val; - uint32_t btmBufferCnt_val; - uint32_t outputEnable_val; - CTR_Code retval = ((CanTalonSRX*)handle)->GetMotionProfileStatus(flags_val, profileSlotSelect_val, targPos_val, targVel_val, topBufferRemaining_val, topBufferCnt_val, btmBufferCnt_val, outputEnable_val); - *flags = (int)flags_val; - *profileSlotSelect = (int)profileSlotSelect_val; - *targPos = (int)targPos_val; - *targVel = (int)targVel_val; - *topBufferRemaining = (int)topBufferRemaining_val; - *topBufferCnt = (int)topBufferCnt_val; - *btmBufferCnt = (int)btmBufferCnt_val; - *outputEnable = (int)outputEnable_val; - return retval; -} -CTR_Code c_TalonSRX_GetFault_OverTemp(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_OverTemp(*param); -} -CTR_Code c_TalonSRX_GetFault_UnderVoltage(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_UnderVoltage(*param); -} -CTR_Code c_TalonSRX_GetFault_ForLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_ForLim(*param); -} -CTR_Code c_TalonSRX_GetFault_RevLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_RevLim(*param); -} -CTR_Code c_TalonSRX_GetFault_HardwareFailure(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_HardwareFailure(*param); -} -CTR_Code c_TalonSRX_GetFault_ForSoftLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_ForSoftLim(*param); -} -CTR_Code c_TalonSRX_GetFault_RevSoftLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFault_RevSoftLim(*param); -} -CTR_Code c_TalonSRX_GetStckyFault_OverTemp(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetStckyFault_OverTemp(*param); -} -CTR_Code c_TalonSRX_GetStckyFault_UnderVoltage(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetStckyFault_UnderVoltage(*param); -} -CTR_Code c_TalonSRX_GetStckyFault_ForLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetStckyFault_ForLim(*param); -} -CTR_Code c_TalonSRX_GetStckyFault_RevLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetStckyFault_RevLim(*param); -} -CTR_Code c_TalonSRX_GetStckyFault_ForSoftLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetStckyFault_ForSoftLim(*param); -} -CTR_Code c_TalonSRX_GetStckyFault_RevSoftLim(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetStckyFault_RevSoftLim(*param); -} -CTR_Code c_TalonSRX_GetAppliedThrottle(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetAppliedThrottle(*param); -} -CTR_Code c_TalonSRX_GetCloseLoopErr(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetCloseLoopErr(*param); -} -CTR_Code c_TalonSRX_GetFeedbackDeviceSelect(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFeedbackDeviceSelect(*param); -} -CTR_Code c_TalonSRX_GetModeSelect(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetModeSelect(*param); -} -CTR_Code c_TalonSRX_GetLimitSwitchEn(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetLimitSwitchEn(*param); -} -CTR_Code c_TalonSRX_GetLimitSwitchClosedFor(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetLimitSwitchClosedFor(*param); -} -CTR_Code c_TalonSRX_GetLimitSwitchClosedRev(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetLimitSwitchClosedRev(*param); -} -CTR_Code c_TalonSRX_GetSensorPosition(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetSensorPosition(*param); -} -CTR_Code c_TalonSRX_GetSensorVelocity(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetSensorVelocity(*param); -} -CTR_Code c_TalonSRX_GetCurrent(void *handle, double *param) -{ - return ((CanTalonSRX*)handle)->GetCurrent(*param); -} -CTR_Code c_TalonSRX_GetBrakeIsEnabled(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetBrakeIsEnabled(*param); -} -CTR_Code c_TalonSRX_GetEncPosition(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetEncPosition(*param); -} -CTR_Code c_TalonSRX_GetEncVel(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetEncVel(*param); -} -CTR_Code c_TalonSRX_GetEncIndexRiseEvents(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetEncIndexRiseEvents(*param); -} -CTR_Code c_TalonSRX_GetQuadApin(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetQuadApin(*param); -} -CTR_Code c_TalonSRX_GetQuadBpin(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetQuadBpin(*param); -} -CTR_Code c_TalonSRX_GetQuadIdxpin(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetQuadIdxpin(*param); -} -CTR_Code c_TalonSRX_GetAnalogInWithOv(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetAnalogInWithOv(*param); -} -CTR_Code c_TalonSRX_GetAnalogInVel(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetAnalogInVel(*param); -} -CTR_Code c_TalonSRX_GetTemp(void *handle, double *param) -{ - return ((CanTalonSRX*)handle)->GetTemp(*param); -} -CTR_Code c_TalonSRX_GetBatteryV(void *handle, double *param) -{ - return ((CanTalonSRX*)handle)->GetBatteryV(*param); -} -CTR_Code c_TalonSRX_GetResetCount(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetResetCount(*param); -} -CTR_Code c_TalonSRX_GetResetFlags(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetResetFlags(*param); -} -CTR_Code c_TalonSRX_GetFirmVers(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetFirmVers(*param); -} -CTR_Code c_TalonSRX_GetPulseWidthPosition(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetPulseWidthPosition(*param); -} -CTR_Code c_TalonSRX_GetPulseWidthVelocity(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetPulseWidthVelocity(*param); -} -CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetPulseWidthRiseToRiseUs(*param); -} -CTR_Code c_TalonSRX_GetActTraj_IsValid(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetActTraj_IsValid(*param); -} -CTR_Code c_TalonSRX_GetActTraj_ProfileSlotSelect(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetActTraj_ProfileSlotSelect(*param); -} -CTR_Code c_TalonSRX_GetActTraj_VelOnly(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetActTraj_VelOnly(*param); -} -CTR_Code c_TalonSRX_GetActTraj_IsLast(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetActTraj_IsLast(*param); -} -CTR_Code c_TalonSRX_GetOutputType(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetOutputType(*param); -} -CTR_Code c_TalonSRX_GetHasUnderrun(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetHasUnderrun(*param); -} -CTR_Code c_TalonSRX_GetIsUnderrun(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetIsUnderrun(*param); -} -CTR_Code c_TalonSRX_GetNextID(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetNextID(*param); -} -CTR_Code c_TalonSRX_GetBufferIsFull(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetBufferIsFull(*param); -} -CTR_Code c_TalonSRX_GetCount(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetCount(*param); -} -CTR_Code c_TalonSRX_GetActTraj_Velocity(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetActTraj_Velocity(*param); -} -CTR_Code c_TalonSRX_GetActTraj_Position(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetActTraj_Position(*param); -} -CTR_Code c_TalonSRX_SetDemand(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetDemand(param); -} -CTR_Code c_TalonSRX_SetOverrideLimitSwitchEn(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetOverrideLimitSwitchEn(param); -} -CTR_Code c_TalonSRX_SetFeedbackDeviceSelect(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetFeedbackDeviceSelect(param); -} -CTR_Code c_TalonSRX_SetRevMotDuringCloseLoopEn(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetRevMotDuringCloseLoopEn(param); -} -CTR_Code c_TalonSRX_SetOverrideBrakeType(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetOverrideBrakeType(param); -} -CTR_Code c_TalonSRX_SetModeSelect(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetModeSelect(param); -} -CTR_Code c_TalonSRX_SetProfileSlotSelect(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetProfileSlotSelect(param); -} -CTR_Code c_TalonSRX_SetRampThrottle(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetRampThrottle(param); -} -CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param) -{ - return ((CanTalonSRX*)handle)->SetRevFeedbackSensor(param); -} -} diff --git a/wpilibc/athena/include/CANTalon.h b/wpilibc/athena/include/CANTalon.h deleted file mode 100644 index fb2e33d37c..0000000000 --- a/wpilibc/athena/include/CANTalon.h +++ /dev/null @@ -1,583 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2014-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 - -#include -#include - -#include "CANSpeedController.h" -#include "HAL/CanTalonSRX.h" -#include "LiveWindow/LiveWindowSendable.h" -#include "MotorSafetyHelper.h" -#include "PIDInterface.h" -#include "PIDOutput.h" -#include "PIDSource.h" -#include "SafePWM.h" -#include "tables/ITable.h" - -/** - * CTRE Talon SRX Speed Controller with CAN Control - */ -class CANTalon : public MotorSafety, - public CANSpeedController, - public ErrorBase, - public LiveWindowSendable, - public ITableListener, - public PIDSource, - public PIDInterface { - public: - enum FeedbackDevice { - QuadEncoder = 0, - AnalogPot = 2, - AnalogEncoder = 3, - EncRising = 4, - EncFalling = 5, - CtreMagEncoder_Relative = 6, //!< Cross The Road Electronics Magnetic - //! Encoder in Absolute/PulseWidth Mode - CtreMagEncoder_Absolute = 7, //!< Cross The Road Electronics Magnetic - //! Encoder in Relative/Quadrature Mode - PulseWidth = 8, - }; - /** - * Depending on the sensor type, Talon can determine if sensor is plugged in - * ot not. - */ - enum FeedbackDeviceStatus { - FeedbackStatusUnknown = 0, //!< Sensor status could not be determined. Not - //! all sensors can do this. - FeedbackStatusPresent = 1, //!< Sensor is present and working okay. - FeedbackStatusNotPresent = - 2, //!< Sensor is not present, not plugged in, not powered, etc... - }; - enum StatusFrameRate { - StatusFrameRateGeneral = 0, - StatusFrameRateFeedback = 1, - StatusFrameRateQuadEncoder = 2, - StatusFrameRateAnalogTempVbat = 3, - StatusFrameRatePulseWidthMeas = 4, - }; - /** - * Enumerated types for Motion Control Set Values. - * When in Motion Profile control mode, these constants are paseed - * into set() to manipulate the motion profile executer. - * When changing modes, be sure to read the value back using - * getMotionProfileStatus() to ensure changes in output take effect before - * performing buffering actions. - * Disable will signal Talon to put motor output into neutral drive. - * Talon will stop processing motion profile points. This means the buffer - * is effectively disconnected from the executer, allowing the robot to - * gracefully clear and push new traj points. isUnderrun will get cleared. - * The active trajectory is also cleared. - * Enable will signal Talon to pop a trajectory point from it's buffer and - * process it. If the active trajectory is empty, Talon will shift in the - * next point. If the active traj is empty, and so is the buffer, the motor - * drive is neutral and isUnderrun is set. When active traj times out, and - * buffer has at least one point, Talon shifts in next one, and isUnderrun - * is cleared. When active traj times out, and buffer is empty, Talon - * keeps processing active traj and sets IsUnderrun. - * Hold will signal Talon keep processing the active trajectory indefinitely. - * If the active traj is cleared, Talon will neutral motor drive. Otherwise - * Talon will keep processing the active traj but it will not shift in - * points from the buffer. This means the buffer is effectively - * disconnected from the executer, allowing the robot to gracefully clear - * and push new traj points. isUnderrun is set if active traj is empty, - * otherwise it is cleared. isLast signal is also cleared. - * - * Typical workflow: - * set(Disable), - * Confirm Disable takes effect, - * clear buffer and push buffer points, - * set(Enable) when enough points have been pushed to ensure no underruns, - * wait for MP to finish or decide abort, - * If MP finished gracefully set(Hold) to hold position servo and disconnect - * buffer, - * If MP is being aborted set(Disable) to neutral the motor and disconnect - * buffer, - * Confirm mode takes effect, - * clear buffer and push buffer points, and rinse-repeat. - */ - enum SetValueMotionProfile { - SetValueMotionProfileDisable = 0, - SetValueMotionProfileEnable = 1, - SetValueMotionProfileHold = 2, - }; - /** - * Motion Profile Trajectory Point - * This is simply a data transer object. - */ - struct TrajectoryPoint { - double position; //!< The position to servo to. - double velocity; //!< The velocity to feed-forward. - - /** - * Time in milliseconds to process this point. - * Value should be between 1ms and 255ms. If value is zero - * then Talon will default to 1ms. If value exceeds 255ms API will cap it. - */ - int timeDurMs; - /** - * Which slot to get PIDF gains. - * PID is used for position servo. - * F is used as the Kv constant for velocity feed-forward. - * Typically this is hardcoded to the a particular slot, but you are free - * gain schedule if need be. - */ - int profileSlotSelect; - /** - * Set to true to only perform the velocity feed-forward and not perform - * position servo. This is useful when learning how the position servo - * changes the motor response. The same could be accomplish by clearing the - * PID gains, however this is synchronous the streaming, and doesn't require - * restoing - * gains when finished. - * - * Additionaly setting this basically gives you direct control of the motor - * output - * since motor output = targetVelocity X Kv, where Kv is our Fgain. - * This means you can also scheduling straight-throttle curves without - * relying on - * a sensor. - */ - bool velocityOnly; - /** - * Set to true to signal Talon that this is the final point, so do not - * attempt to pop another trajectory point from out of the Talon buffer. - * Instead continue processing this way point. Typically the velocity - * member variable should be zero so that the motor doesn't spin - * indefinitely. - */ - bool isLastPoint; - /** - * Set to true to signal Talon to zero the selected sensor. - * When generating MPs, one simple method is to make the first target - * position zero, - * and the final target position the target distance from the current - * position. - * Then when you fire the MP, the current position gets set to zero. - * If this is the intent, you can set zeroPos on the first trajectory - * point. - * - * Otherwise you can leave this false for all points, and offset the - * positions - * of all trajectory points so they are correct. - */ - bool zeroPos; - }; - /** - * Motion Profile Status - * This is simply a data transer object. - */ - struct MotionProfileStatus { - /** - * The available empty slots in the trajectory buffer. - * - * The robot API holds a "top buffer" of trajectory points, so your - * applicaion can dump several points at once. The API will then stream - * them into the Talon's low-level buffer, allowing the Talon to act on - * them. - */ - int topBufferRem; - /** - * The number of points in the top trajectory buffer. - */ - int topBufferCnt; - /** - * The number of points in the low level Talon buffer. - */ - int btmBufferCnt; - /** - * Set if isUnderrun ever gets set. - * Only is cleared by clearMotionProfileHasUnderrun() to ensure - * robot logic can react or instrument it. - * @see clearMotionProfileHasUnderrun() - */ - bool hasUnderrun; - /** - * This is set if Talon needs to shift a point from its buffer into - * the active trajectory point however the buffer is empty. This gets - * cleared automatically when is resolved. - */ - bool isUnderrun; - /** - * True if the active trajectory point has not empty, false otherwise. - * The members in activePoint are only valid if this signal is set. - */ - bool activePointValid; - /** - * The number of points in the low level Talon buffer. - */ - TrajectoryPoint activePoint; - /** - * The current output mode of the motion profile executer (disabled, - * enabled, or hold). - * When changing the set() value in MP mode, it's important to check this - * signal to confirm the change takes effect before interacting with the - * top buffer. - */ - SetValueMotionProfile outputEnable; - }; - explicit CANTalon(int deviceNumber); - explicit CANTalon(int deviceNumber, int controlPeriodMs); - DEFAULT_MOVE_CONSTRUCTOR(CANTalon); - virtual ~CANTalon(); - - // PIDOutput interface - void PIDWrite(float output) override; - - // PIDSource interface - double PIDGet() override; - - // MotorSafety interface - void SetExpiration(float timeout) override; - float GetExpiration() const override; - bool IsAlive() const override; - void StopMotor() override; - void SetSafetyEnabled(bool enabled) override; - bool IsSafetyEnabled() const override; - void GetDescription(std::ostringstream& desc) const override; - - // CANSpeedController interface - float Get() const override; - void Set(float value) override; - void Reset() override; - void SetSetpoint(float value) override; - void Disable() override; - virtual void EnableControl(); - void Enable() override; - void SetP(double p) override; - void SetI(double i) override; - void SetD(double d) override; - void SetF(double f); - void SetIzone(int iz); - void SetPID(double p, double i, double d) override; - virtual void SetPID(double p, double i, double d, double f); - double GetP() const override; - double GetI() const override; - double GetD() const override; - virtual double GetF() const; - bool IsModePID(CANSpeedController::ControlMode mode) const override; - float GetBusVoltage() const override; - float GetOutputVoltage() const override; - float GetOutputCurrent() const override; - float GetTemperature() const override; - void SetPosition(double pos); - double GetPosition() const override; - double GetSpeed() const override; - virtual int GetClosedLoopError() const; - virtual void SetAllowableClosedLoopErr(int allowableCloseLoopError); - virtual int GetAnalogIn() const; - virtual void SetAnalogPosition(int newPosition); - virtual int GetAnalogInRaw() const; - virtual int GetAnalogInVel() const; - virtual int GetEncPosition() const; - virtual void SetEncPosition(int); - virtual int GetEncVel() const; - int GetPinStateQuadA() const; - int GetPinStateQuadB() const; - int GetPinStateQuadIdx() const; - int IsFwdLimitSwitchClosed() const; - int IsRevLimitSwitchClosed() const; - int GetNumberOfQuadIdxRises() const; - void SetNumberOfQuadIdxRises(int rises); - virtual int GetPulseWidthPosition() const; - virtual void SetPulseWidthPosition(int newpos); - virtual int GetPulseWidthVelocity() const; - virtual int GetPulseWidthRiseToFallUs() const; - virtual int GetPulseWidthRiseToRiseUs() const; - virtual FeedbackDeviceStatus IsSensorPresent( - FeedbackDevice feedbackDevice) const; - bool GetForwardLimitOK() const override; - bool GetReverseLimitOK() const override; - uint16_t GetFaults() const override; - uint16_t GetStickyFaults() const; - void ClearStickyFaults(); - void SetVoltageRampRate(double rampRate) override; - virtual void SetVoltageCompensationRampRate(double rampRate); - int GetFirmwareVersion() const override; - void ConfigNeutralMode(NeutralMode mode) override; - void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override; - void ConfigPotentiometerTurns(uint16_t turns) override; - void ConfigSoftPositionLimits(double forwardLimitPosition, - double reverseLimitPosition) override; - void DisableSoftPositionLimits() override; - void ConfigLimitMode(LimitMode mode) override; - void ConfigForwardLimit(double forwardLimitPosition) override; - void ConfigReverseLimit(double reverseLimitPosition) override; - void ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, - bool bReverseLimitSwitchEn); - void ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn); - void ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn); - /** - * Change the fwd limit switch setting to normally open or closed. - * Talon will disable momentarilly if the Talon's current setting - * is dissimilar to the caller's requested setting. - * - * Since Talon saves setting to flash this should only affect - * a given Talon initially during robot install. - * - * @param normallyOpen true for normally open. false for normally closed. - */ - void ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen); - /** - * Change the rev limit switch setting to normally open or closed. - * Talon will disable momentarilly if the Talon's current setting - * is dissimilar to the caller's requested setting. - * - * Since Talon saves setting to flash this should only affect - * a given Talon initially during robot install. - * - * @param normallyOpen true for normally open. false for normally closed. - */ - void ConfigRevLimitSwitchNormallyOpen(bool normallyOpen); - void ConfigMaxOutputVoltage(double voltage) override; - void ConfigPeakOutputVoltage(double forwardVoltage, double reverseVoltage); - void ConfigNominalOutputVoltage(double forwardVoltage, double reverseVoltage); - /** - * Enables Talon SRX to automatically zero the Sensor Position whenever an - * edge is detected on the index signal. - * - * @param enable boolean input, pass true to enable feature or false to - * disable. - * @param risingEdge boolean input, pass true to clear the position on rising - * edge, pass false to clear the position on falling edge. - */ - void EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge); - void ConfigSetParameter(int paramEnum, double value); - bool GetParameter(int paramEnum, double& dvalue) const; - - void ConfigFaultTime(float faultTime) override; - virtual void SetControlMode(ControlMode mode); - void SetFeedbackDevice(FeedbackDevice device); - void SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs); - virtual ControlMode GetControlMode() const; - void SetSensorDirection(bool reverseSensor); - void SetClosedLoopOutputDirection(bool reverseOutput); - void SetCloseLoopRampRate(double rampRate); - void SelectProfileSlot(int slotIdx); - int GetIzone() const; - int GetIaccum() const; - void ClearIaccum(); - int GetBrakeEnableDuringNeutral() const; - - bool IsControlEnabled() const; - bool IsEnabled() const override; - double GetSetpoint() const override; - - /** - * 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(int periodMs); - - /** - * Clear the buffered motion profile in both Talon RAM (bottom), and in the - * API (top). Be sure to check GetMotionProfileStatus() to know when the - * buffer is actually cleared. - */ - 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. - */ - int GetMotionProfileTopLevelBufferCount(); - - /** - * Push another trajectory point into the top level buffer (which is emptied - * into the Talon's bottom buffer as room allows). - * - * @param trajPt the trajectory point to insert into buffer. - * @return true if trajectory point push ok. CTR_BufferFull if buffer is full - * due to kMotionProfileTopBufferCapacity. - */ - bool PushMotionProfileTrajectory(const TrajectoryPoint& trajPt); - - /** - * @return true if api-level (top) buffer is full. - */ - bool IsMotionProfileTopLevelBufferFull(); - - /** - * 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] motionProfileStatus contains all progress information on the - * currently running MP. - */ - void GetMotionProfileStatus(MotionProfileStatus& motionProfileStatus); - - /** - * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is - * ready for another point, but the low level buffer is empty. - * - * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set - * until Robot Application clears it with this routine, which ensures Robot - * Application gets a chance to instrument or react. Caller could also check - * the isUnderrun flag which automatically clears when fault condition is - * removed. - */ - void ClearMotionProfileHasUnderrun(); - - // LiveWindow stuff. - void ValueChanged(ITable* source, llvm::StringRef key, - std::shared_ptr value, bool isNew) override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; - - // SpeedController overrides - void SetInverted(bool isInverted) override; - bool GetInverted() const override; - - private: - // Values for various modes as is sent in the CAN packets for the Talon. - enum TalonControlMode { - kThrottle = 0, - kFollowerMode = 5, - kVoltageMode = 4, - kPositionMode = 1, - kSpeedMode = 2, - kCurrentMode = 3, - kMotionProfileMode = 6, - kDisabled = 15 - }; - - int m_deviceNumber; - std::unique_ptr m_impl; - std::unique_ptr m_safetyHelper; - int m_profile = 0; // Profile from CANTalon to use. Set to zero until we - // can actually test this. - - bool m_controlEnabled = true; - bool m_stopped = false; - ControlMode m_controlMode = kPercentVbus; - TalonControlMode m_sendMode; - - double m_setPoint = 0; - /** - * Encoder CPR, counts per rotations, also called codes per revoluion. - * Default value of zero means the API behaves as it did during the 2015 - * season, each position unit is a single pulse and there are four pulses per - * count (4X). Caller can use ConfigEncoderCodesPerRev to set the quadrature - * encoder CPR. - */ - int m_codesPerRev = 0; - /** - * Number of turns per rotation. For example, a 10-turn pot spins ten full - * rotations from a wiper voltage of zero to 3.3 volts. Therefore knowing - * the number of turns a full voltage sweep represents is necessary for - * calculating rotations and velocity. A default value of zero means the API - * behaves as it did during the 2015 season, there are 1024 position units - * from zero to 3.3V. - */ - int m_numPotTurns = 0; - /** - * Although the Talon handles feedback selection, caching the feedback - * selection is helpful at the API level for scaling into rotations and RPM. - */ - FeedbackDevice m_feedbackDevice = QuadEncoder; - - static constexpr int kDelayForSolicitedSignalsUs = 4000; - /** - * @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon - * allows multiple sensors to be attached simultaneously, - * caller must specify which sensor to lookup. - * @return The number of native Talon units per rotation of the selected - * sensor. Zero if the necessary sensor information is not available. - * @see ConfigEncoderCodesPerRev - * @see ConfigPotentiometerTurns - */ - double GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup) const; - /** - * Fixup the sendMode so Set() serializes the correct demand value. - * Also fills the modeSelecet in the control frame to disabled. - * @param mode Control mode to ultimately enter once user calls Set(). - * @see Set() - */ - void ApplyControlMode(CANSpeedController::ControlMode mode); - /** - * @param fullRotations double precision value representing number of - * rotations of selected feedback sensor. If user has - * never called the config routine for the selected - * sensor, then the caller is likely passing rotations - * in engineering units already, in which case it is - * returned as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return fullRotations in native engineering units of the Talon SRX - * firmware. - */ - int ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, - double fullRotations) const; - /** - * @param rpm double precision value representing number of rotations per - * minute of selected feedback sensor. If user has never called - * the config routine for the selected sensor, then the caller is - * likely passing rotations in engineering units already, in which - * case it is returned as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return sensor velocity in native engineering units of the Talon SRX - * firmware. - */ - int ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, double rpm) const; - /** - * @param nativePos integral position of the feedback sensor in native - * Talon SRX units. If user has never called the config - * routine for the selected sensor, then the return will be - * in TALON SRX units as well to match the behavior in the - * 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision number of rotations, unless config was never - * performed. - */ - double ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, - int nativePos) const; - /** - * @param nativeVel integral velocity of the feedback sensor in native - * Talon SRX units. If user has never called the config - * routine for the selected sensor, then the return will be - * in TALON SRX units as well to match the behavior in the - * 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision of sensor velocity in RPM, unless config was never - * performed. - */ - double ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, int nativeVel) const; - - // LiveWindow stuff. - std::shared_ptr m_table; - /** - * Flips the output direction during open-loop modes like percent - * voltage, or certain closed loop modes like speed/current mode. - */ - bool m_isInverted = false; - - HasBeenMoved m_hasBeenMoved; -}; diff --git a/wpilibc/athena/include/WPILib.h b/wpilibc/athena/include/WPILib.h index 8f0002f047..c45d506c6e 100644 --- a/wpilibc/athena/include/WPILib.h +++ b/wpilibc/athena/include/WPILib.h @@ -25,7 +25,6 @@ #include "Buttons/JoystickButton.h" #include "Buttons/NetworkButton.h" #include "CANJaguar.h" -#include "CANTalon.h" #include "CameraServer.h" #include "Commands/Command.h" #include "Commands/CommandGroup.h" diff --git a/wpilibc/athena/src/CANTalon.cpp b/wpilibc/athena/src/CANTalon.cpp deleted file mode 100644 index 7fafe719e6..0000000000 --- a/wpilibc/athena/src/CANTalon.cpp +++ /dev/null @@ -1,2090 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2014-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. */ -/*----------------------------------------------------------------------------*/ - -#include "CANTalon.h" - -#include -#include -#include - -#include "HAL/HAL.h" -#include "LiveWindow/LiveWindow.h" -#include "WPIErrors.h" - -/** - * Number of adc engineering units per 0 to 3.3V sweep. - * This is necessary for scaling Analog Position in rotations/RPM. - */ -const double kNativeAdcUnitsPerRotation = 1024.0; -/** - * Number of pulse width engineering units per full rotation. - * This is necessary for scaling Pulse Width Decoded Position in rotations/RPM. - */ -const double kNativePwdUnitsPerRotation = 4096.0; -/** - * Number of minutes per 100ms unit. Useful for scaling velocities - * measured by Talon's 100ms timebase to rotations per minute. - */ -const double kMinutesPer100msUnit = 1.0 / 600.0; - -constexpr int CANTalon::kDelayForSolicitedSignalsUs; - -/** - * 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)) { - ApplyControlMode(m_controlMode); - m_impl->SetProfileSlotSelect(m_profile); - LiveWindow::GetInstance()->AddActuator("CANTalon", m_deviceNumber, this); -} -/** - * Constructor for the CANTalon device. - * - * @param deviceNumber The CAN ID of the Talon SRX - * @param controlPeriodMs The period in ms to send the CAN control frame. - * Period is bounded to [1ms,95ms]. - */ -CANTalon::CANTalon(int deviceNumber, int controlPeriodMs) - : m_deviceNumber(deviceNumber), - m_impl(new CanTalonSRX(deviceNumber, controlPeriodMs)), - m_safetyHelper(new MotorSafetyHelper(this)) { - ApplyControlMode(m_controlMode); - m_impl->SetProfileSlotSelect(m_profile); - LiveWindow::GetInstance()->AddActuator("CANTalon", m_deviceNumber, this); -} - -CANTalon::~CANTalon() { - if (m_table != nullptr) m_table->RemoveTableListener(this); - if (m_hasBeenMoved) return; - Disable(); -} - -/** - * 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"); - } -} - -/** - * Retrieve the current sensor value. Equivalent to Get(). - * - * @return The current sensor value of the Talon. - */ -double CANTalon::PIDGet() { return Get(); } - -/** - * Gets the current status of the Talon (usually a sensor value). - * - * In Current mode: returns output current. - * In Speed mode: returns current speed. - * In Position mode: returns current sensor position. - * In PercentVbus and Follower modes: returns current applied throttle. - * - * @return The current sensor value of the Talon. - */ -float CANTalon::Get() const { - int value; - switch (m_controlMode) { - case kVoltage: - return GetOutputVoltage(); - case kCurrent: - return GetOutputCurrent(); - case kSpeed: - m_impl->GetSensorVelocity(value); - return ScaleNativeUnitsToRpm(m_feedbackDevice, value); - case kPosition: - m_impl->GetSensorPosition(value); - return ScaleNativeUnitsToRotations(m_feedbackDevice, value); - case kPercentVbus: - case kFollower: - default: - m_impl->GetAppliedThrottle(value); - return static_cast(value) / 1023.0; - } -} - -/** - * Sets the appropriate output on the talon, depending on the mode. - * - * In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped. - * In Voltage mode, output value is in volts. - * In Current mode, output value is in amperes. - * In Speed mode, output value is in position change / 10ms. - * In Position mode, output value is in encoder ticks or an analog value, - * depending on the sensor. - * In Follower mode, the output value is the integer device ID of the talon to - * duplicate. - * - * @param outputValue The setpoint value, as described above. - * @see SelectProfileSlot to choose between the two sets of gains. - */ -void CANTalon::Set(float value) { - /* feed safety helper since caller just updated our output */ - m_safetyHelper->Feed(); - - if (m_stopped) { - EnableControl(); - m_stopped = false; - } - - if (m_controlEnabled) { - m_setPoint = value; /* cache set point for GetSetpoint() */ - CTR_Code status = CTR_OKAY; - switch (m_controlMode) { - case CANSpeedController::kPercentVbus: { - m_impl->Set(m_isInverted ? -value : value); - status = CTR_OKAY; - } break; - case CANSpeedController::kFollower: { - status = m_impl->SetDemand(static_cast(value)); - } break; - case CANSpeedController::kVoltage: { - // Voltage is an 8.8 fixed point number. - int volts = static_cast((m_isInverted ? -value : value) * 256); - status = m_impl->SetDemand(volts); - } break; - case CANSpeedController::kSpeed: - /* if the caller has provided scaling info, apply it */ - status = m_impl->SetDemand(ScaleVelocityToNativeUnits( - m_feedbackDevice, m_isInverted ? -value : value)); - break; - case CANSpeedController::kPosition: - status = m_impl->SetDemand( - ScaleRotationsToNativeUnits(m_feedbackDevice, value)); - break; - case CANSpeedController::kCurrent: { - double milliamperes = (m_isInverted ? -value : value) * 1000.0; /* mA*/ - status = m_impl->SetDemand(milliamperes); - } break; - case CANSpeedController::kMotionProfile: { - status = m_impl->SetDemand(static_cast(value)); - } break; - default: - wpi_setWPIErrorWithContext( - IncompatibleMode, - "The CAN Talon does not support this control mode."); - break; - } - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - - status = m_impl->SetModeSelect(m_sendMode); - - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - } -} - -/** - * Sets the setpoint to value. Equivalent to Set(). - */ -void CANTalon::SetSetpoint(float value) { Set(value); } - -/** - * Resets the integral term and disables the controller. - */ -void CANTalon::Reset() { - ClearIaccum(); - Disable(); -} - -/** - * Disables control of the talon, causing the motor to brake or coast - * depending on its mode (see the Talon SRX Software Reference manual - * for more information). - */ -void CANTalon::Disable() { - m_impl->SetModeSelect(static_cast(CANTalon::kDisabled)); - m_controlEnabled = false; -} - -/** - * Enables control of the Talon, allowing the motor to move. - */ -void CANTalon::EnableControl() { - SetControlMode(m_controlMode); - m_controlEnabled = true; -} - -/** - * Enables control of the Talon, allowing the motor to move. - */ -void CANTalon::Enable() { EnableControl(); } - -/** - * @return Whether the Talon is currently enabled. - */ -bool CANTalon::IsControlEnabled() const { return m_controlEnabled; } - -/** - * @return Whether the Talon is currently enabled. - */ -bool CANTalon::IsEnabled() const { return IsControlEnabled(); } - -/** - * @param p Proportional constant to use in PID loop. - * @see SelectProfileSlot to choose between the two sets of gains. - */ -void CANTalon::SetP(double p) { - CTR_Code status = m_impl->SetPgain(m_profile, p); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Set the integration constant of the currently selected profile. - * - * @param i Integration constant for the currently selected PID profile. - * @see SelectProfileSlot to choose between the two sets of gains. - */ -void CANTalon::SetI(double i) { - CTR_Code status = m_impl->SetIgain(m_profile, i); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Set the derivative constant of the currently selected profile. - * - * @param d Derivative constant for the currently selected PID profile. - * @see SelectProfileSlot to choose between the two sets of gains. - */ -void CANTalon::SetD(double d) { - CTR_Code status = m_impl->SetDgain(m_profile, d); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Set the feedforward value of the currently selected profile. - * - * @param f Feedforward constant for the currently selected PID profile. - * @see SelectProfileSlot to choose between the two sets of gains. - */ -void CANTalon::SetF(double f) { - CTR_Code status = m_impl->SetFgain(m_profile, f); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Set the Izone to a nonzero value to auto clear the integral accumulator - * when the absolute value of CloseLoopError exceeds Izone. - * - * @see SelectProfileSlot to choose between the two sets of gains. - */ -void CANTalon::SetIzone(int iz) { - CTR_Code status = m_impl->SetIzone(m_profile, iz); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * SRX has two available slots for PID. - * @param slotIdx one or zero depending on which slot caller wants. - */ -void CANTalon::SelectProfileSlot(int slotIdx) { - m_profile = (slotIdx == 0) ? 0 : 1; /* only get two slots for now */ - CTR_Code status = m_impl->SetProfileSlotSelect(m_profile); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Sets control values for closed loop control. - * - * @param p Proportional constant. - * @param i Integration constant. - * @param d Differential constant. - * - * This function does not modify F-gain. Considerable passing a zero for f - * using the four-parameter function. - */ -void CANTalon::SetPID(double p, double i, double d) { - SetP(p); - SetI(i); - SetD(d); -} - -/** - * Sets control values for closed loop control. - * - * @param p Proportional constant. - * @param i Integration constant. - * @param d Differential constant. - * @param f Feedforward constant. - */ -void CANTalon::SetPID(double p, double i, double d, double f) { - SetP(p); - SetI(i); - SetD(d); - SetF(f); -} - -/** - * Select the feedback device to use in closed-loop - */ -void CANTalon::SetFeedbackDevice(FeedbackDevice feedbackDevice) { - /* save the selection so that future setters/getters know which scalars to - * apply - */ - m_feedbackDevice = feedbackDevice; - /* pass feedback to actual CAN frame */ - CTR_Code status = - m_impl->SetFeedbackDeviceSelect(static_cast(feedbackDevice)); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Select the feedback device to use in closed-loop - */ -void CANTalon::SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs) { - CTR_Code status = - m_impl->SetStatusFrameRate(static_cast(stateFrame), periodMs); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Get the current proportional constant. - * - * @return double proportional constant for current profile. - * @see SelectProfileSlot to choose between the two sets of gains. - */ -double CANTalon::GetP() const { - CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_P - : CanTalonSRX::eProfileParamSlot0_P; - // Update the info in m_impl. - CTR_Code status = m_impl->RequestParam(param); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - // small yield for getting response - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - double p; - status = m_impl->GetPgain(m_profile, p); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return p; -} - -/** - * TODO documentation (see CANJaguar.cpp) - * @see SelectProfileSlot to choose between the two sets of gains. - */ -double CANTalon::GetI() const { - CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_I - : CanTalonSRX::eProfileParamSlot0_I; - // Update the info in m_impl. - CTR_Code status = m_impl->RequestParam(param); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - // small yield for getting response - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - - double i; - status = m_impl->GetIgain(m_profile, i); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return i; -} - -/** - * TODO documentation (see CANJaguar.cpp) - * @see SelectProfileSlot to choose between the two sets of gains. - */ -double CANTalon::GetD() const { - CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_D - : CanTalonSRX::eProfileParamSlot0_D; - // Update the info in m_impl. - CTR_Code status = m_impl->RequestParam(param); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - // small yield for getting response - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - double d; - status = m_impl->GetDgain(m_profile, d); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return d; -} - -/** - * - * @see SelectProfileSlot to choose between the two sets of gains. - */ -double CANTalon::GetF() const { - CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_F - : CanTalonSRX::eProfileParamSlot0_F; - // Update the info in m_impl. - CTR_Code status = m_impl->RequestParam(param); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - - // small yield for getting response - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - double f; - status = m_impl->GetFgain(m_profile, f); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return f; -} - -/** - * @see SelectProfileSlot to choose between the two sets of gains. - */ -int CANTalon::GetIzone() const { - CanTalonSRX::param_t param = m_profile - ? CanTalonSRX::eProfileParamSlot1_IZone - : CanTalonSRX::eProfileParamSlot0_IZone; - // Update the info in m_impl. - CTR_Code status = m_impl->RequestParam(param); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - - int iz; - status = m_impl->GetIzone(m_profile, iz); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return iz; -} - -/** - * @return the current setpoint; ie, whatever was last passed to Set(). - */ -double CANTalon::GetSetpoint() const { return m_setPoint; } - -/** - * Returns the voltage coming in from the battery. - * - * @return The input voltage in volts. - */ -float CANTalon::GetBusVoltage() const { - double voltage; - CTR_Code status = m_impl->GetBatteryV(voltage); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return voltage; -} - -/** - * @return The voltage being output by the Talon, in Volts. - */ -float CANTalon::GetOutputVoltage() const { - int throttle11; - CTR_Code status = m_impl->GetAppliedThrottle(throttle11); - float voltage = GetBusVoltage() * (static_cast(throttle11) / 1023.0); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return voltage; -} - -/** - * Returns the current going through the Talon, in Amperes. - */ -float CANTalon::GetOutputCurrent() const { - double current; - - CTR_Code status = m_impl->GetCurrent(current); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - - return current; -} - -/** - * Returns temperature of Talon, in degrees Celsius. - */ -float CANTalon::GetTemperature() const { - double temp; - - CTR_Code status = m_impl->GetTemp(temp); - if (temp != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return temp; -} - -/** - * Set the position value of the selected sensor. This is useful for zero-ing - * quadrature encoders. - * - * Continuous sensors (like analog encoderes) can also partially be set (the - * portion of the postion based on overflows). - */ -void CANTalon::SetPosition(double pos) { - int nativePos = ScaleRotationsToNativeUnits(m_feedbackDevice, pos); - CTR_Code status = m_impl->SetSensorPosition(nativePos); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - * - * @return The position of the sensor currently providing feedback. - * When using analog sensors, 0 units corresponds to 0V, 1023 - * units corresponds to 3.3V. - * When using an analog encoder (wrapping around 1023 => 0 is - * possible) the units are still 3.3V per 1023 units. - * When using quadrature, each unit is a quadrature edge (4X) mode. - */ -double CANTalon::GetPosition() const { - int position; - CTR_Code status = m_impl->GetSensorPosition(position); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return ScaleNativeUnitsToRotations(m_feedbackDevice, position); -} - -/** - * If sensor and motor are out of phase, sensor can be inverted - * (position and velocity multiplied by -1). - * @see GetPosition and @see GetSpeed. - */ -void CANTalon::SetSensorDirection(bool reverseSensor) { - CTR_Code status = m_impl->SetRevFeedbackSensor(reverseSensor ? 1 : 0); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Flips the sign (multiplies by negative one) the throttle values going into - * the motor on the talon in closed loop modes. Typically the application - * should use SetSensorDirection to keep sensor and motor in phase. - * @see SetSensorDirection - * However this routine is helpful for reversing the motor direction - * when Talon is in slave mode, or when using a single-direction position - * sensor in a closed-loop mode. - * - * @param reverseOutput True if motor output should be flipped; False if not. - */ -void CANTalon::SetClosedLoopOutputDirection(bool reverseOutput) { - CTR_Code status = m_impl->SetRevMotDuringCloseLoopEn(reverseOutput ? 1 : 0); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Returns the current error in the controller. - * - * @return the difference between the setpoint and the sensor value. - */ -int CANTalon::GetClosedLoopError() const { - int error; - /* retrieve the closed loop error in native units */ - CTR_Code status = m_impl->GetCloseLoopErr(error); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return error; -} - -/** - * Set the allowable closed loop error. - * - * @param allowableCloseLoopError allowable closed loop error for selected - * profile. - * - * Units: mA for Curent closed loop. - * Talon Native Units for position and velocity. - */ -void CANTalon::SetAllowableClosedLoopErr(int allowableCloseLoopError) { - /* grab param enum */ - CanTalonSRX::param_t param; - if (m_profile == 1) { - param = CanTalonSRX::eProfileParamSlot1_AllowableClosedLoopErr; - } else { - param = CanTalonSRX::eProfileParamSlot0_AllowableClosedLoopErr; - } - /* send allowable close loop er in native units */ - ConfigSetParameter(param, allowableCloseLoopError); -} - -/** - * TODO documentation (see CANJaguar.cpp) - * - * @returns The speed of the sensor currently providing feedback. - * - * The speed units will be in the sensor's native ticks per 100ms. - * - * For analog sensors, 3.3V corresponds to 1023 units. So a speed of 200 - * equates to ~0.645 dV per 100ms or 6.451 dV per second. If this is an analog - * encoder, that likely means 1.9548 rotations per sec. For quadrature - * encoders, each unit corresponds a quadrature edge (4X). So a 250 count - * encoder will produce 1000 edge events per rotation. An example speed of 200 - * would then equate to 20% of a rotation per 100ms, or 10 rotations per second. - */ -double CANTalon::GetSpeed() const { - int speed; - CTR_Code status = m_impl->GetSensorVelocity(speed); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return ScaleNativeUnitsToRpm(m_feedbackDevice, speed); -} - -/** - * Get the position of whatever is in the analog pin of the Talon, regardless of - * whether it is actually being used for feedback. - * - * @returns The 24bit analog value. The bottom ten bits is the ADC (0 - 1023) - * on the analog pin of the Talon. The upper 14 bits tracks the - * overflows and underflows (continuous sensor). - */ -int CANTalon::GetAnalogIn() const { - int position; - CTR_Code status = m_impl->GetAnalogInWithOv(position); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return position; -} - -void CANTalon::SetAnalogPosition(int newPosition) { - CTR_Code status = m_impl->SetParam(CanTalonSRX::eAinPosition, newPosition); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Get the position of whatever is in the analog pin of the Talon, regardless of - * whether it is actually being used for feedback. - * - * @returns The ADC (0 - 1023) on analog pin of the Talon. - */ -int CANTalon::GetAnalogInRaw() const { return GetAnalogIn() & 0x3FF; } - -/** - * Get the position of whatever is in the analog pin of the Talon, regardless of - * whether it is actually being used for feedback. - * - * @returns The value (0 - 1023) on the analog pin of the Talon. - */ -int CANTalon::GetAnalogInVel() const { - int vel; - CTR_Code status = m_impl->GetAnalogInVel(vel); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return vel; -} - -/** - * Get the position of whatever is in the analog pin of the Talon, regardless of - * whether it is actually being used for feedback. - * - * @returns The value (0 - 1023) on the analog pin of the Talon. - */ -int CANTalon::GetEncPosition() const { - int position; - CTR_Code status = m_impl->GetEncPosition(position); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return position; -} -void CANTalon::SetEncPosition(int newPosition) { - CTR_Code status = m_impl->SetParam(CanTalonSRX::eEncPosition, newPosition); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Get the position of whatever is in the analog pin of the Talon, regardless of - * whether it is actually being used for feedback. - * - * @returns The value (0 - 1023) on the analog pin of the Talon. - */ -int CANTalon::GetEncVel() const { - int vel; - CTR_Code status = m_impl->GetEncVel(vel); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return vel; -} -int CANTalon::GetPulseWidthPosition() const { - int param; - CTR_Code status = m_impl->GetPulseWidthPosition(param); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return param; -} -void CANTalon::SetPulseWidthPosition(int newPosition) { - CTR_Code status = m_impl->SetParam(CanTalonSRX::ePwdPosition, newPosition); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} -int CANTalon::GetPulseWidthVelocity() const { - int param; - CTR_Code status = m_impl->GetPulseWidthVelocity(param); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return param; -} -int CANTalon::GetPulseWidthRiseToFallUs() const { - int param; - CTR_Code status = m_impl->GetPulseWidthRiseToFallUs(param); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return param; -} -int CANTalon::GetPulseWidthRiseToRiseUs() const { - int param; - CTR_Code status = m_impl->GetPulseWidthRiseToRiseUs(param); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - return param; -} - -/** - * @param which feedback sensor to check it if is connected. - * @return status of caller's specified sensor type. - */ -CANTalon::FeedbackDeviceStatus CANTalon::IsSensorPresent( - FeedbackDevice feedbackDevice) const { - FeedbackDeviceStatus retval = FeedbackStatusUnknown; - int param; - /* detecting sensor health depends on which sensor caller cares about */ - switch (feedbackDevice) { - case QuadEncoder: - case AnalogPot: - case AnalogEncoder: - case EncRising: - case EncFalling: - /* no real good way to tell if these sensor - are actually present so return status unknown. */ - break; - case PulseWidth: - case CtreMagEncoder_Relative: - case CtreMagEncoder_Absolute: - /* all of these require pulse width signal to be present. */ - CTR_Code status = m_impl->IsPulseWidthSensorPresent(param); - if (status != CTR_OKAY) { - /* we're not getting status info, signal unknown status */ - } else { - /* param is updated */ - if (param) { - /* pulse signal is present, sensor must be working since it always - generates a pulse waveform.*/ - retval = FeedbackStatusPresent; - } else { - /* no pulse present, sensor disconnected */ - retval = FeedbackStatusNotPresent; - } - } - break; - } - return retval; -} - -/** - * @return IO level of QUADA pin. - */ -int CANTalon::GetPinStateQuadA() const { - int retval; - CTR_Code status = m_impl->GetQuadApin(retval); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return retval; -} - -/** - * @return IO level of QUADB pin. - */ -int CANTalon::GetPinStateQuadB() const { - int retval; - CTR_Code status = m_impl->GetQuadBpin(retval); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return retval; -} - -/** - * @return IO level of QUAD Index pin. - */ -int CANTalon::GetPinStateQuadIdx() const { - int retval; - CTR_Code status = m_impl->GetQuadIdxpin(retval); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return retval; -} - -/** - * @return '1' iff forward limit switch is closed, 0 iff switch is open. - * This function works regardless if limit switch feature is enabled. - */ -int CANTalon::IsFwdLimitSwitchClosed() const { - int retval; - CTR_Code status = m_impl->GetLimitSwitchClosedFor( - retval); /* rename this func, '1' => open, '0' => closed */ - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return retval ? 0 : 1; -} - -/** - * @return '1' iff reverse limit switch is closed, 0 iff switch is open. - * This function works regardless if limit switch feature is enabled. - */ -int CANTalon::IsRevLimitSwitchClosed() const { - int retval; - CTR_Code status = m_impl->GetLimitSwitchClosedRev( - retval); /* rename this func, '1' => open, '0' => closed */ - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return retval ? 0 : 1; -} - -/* - * Simple accessor for tracked rise eventso index pin. - * @return number of rising edges on idx pin. - */ -int CANTalon::GetNumberOfQuadIdxRises() const { - int rises; - CTR_Code status = m_impl->GetEncIndexRiseEvents( - rises); /* rename this func, '1' => open, '0' => closed */ - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return rises; -} - -/* - * @param rises integral value to set into index-rises register. Great way to - * zero the index count. - */ -void CANTalon::SetNumberOfQuadIdxRises(int rises) { - CTR_Code status = m_impl->SetParam( - CanTalonSRX::eEncIndexRiseEvents, - rises); /* rename this func, '1' => open, '0' => closed */ - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -bool CANTalon::GetForwardLimitOK() const { - int limSwit = 0; - int softLim = 0; - CTR_Code status = CTR_OKAY; - status = m_impl->GetFault_ForSoftLim(softLim); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - status = m_impl->GetFault_ForLim(limSwit); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - /* If either fault is asserted, signal caller we are disabled (with false?) */ - return (softLim | limSwit) ? false : true; -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -bool CANTalon::GetReverseLimitOK() const { - int limSwit = 0; - int softLim = 0; - CTR_Code status = CTR_OKAY; - status = m_impl->GetFault_RevSoftLim(softLim); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - status = m_impl->GetFault_RevLim(limSwit); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - /* If either fault is asserted, signal caller we are disabled (with false?) */ - return (softLim | limSwit) ? false : true; -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -uint16_t CANTalon::GetFaults() const { - uint16_t retval = 0; - int val; - CTR_Code status = CTR_OKAY; - - /* temperature */ - val = 0; - status = m_impl->GetFault_OverTemp(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kTemperatureFault : 0; - - /* voltage */ - val = 0; - status = m_impl->GetFault_UnderVoltage(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kBusVoltageFault : 0; - - /* fwd-limit-switch */ - val = 0; - status = m_impl->GetFault_ForLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0; - - /* rev-limit-switch */ - val = 0; - status = m_impl->GetFault_RevLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0; - - /* fwd-soft-limit */ - val = 0; - status = m_impl->GetFault_ForSoftLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0; - - /* rev-soft-limit */ - val = 0; - status = m_impl->GetFault_RevSoftLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kRevSoftLimit : 0; - - return retval; -} - -uint16_t CANTalon::GetStickyFaults() const { - uint16_t retval = 0; - int val; - CTR_Code status = CTR_OKAY; - - /* temperature */ - val = 0; - status = m_impl->GetStckyFault_OverTemp(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kTemperatureFault : 0; - - /* voltage */ - val = 0; - status = m_impl->GetStckyFault_UnderVoltage(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kBusVoltageFault : 0; - - /* fwd-limit-switch */ - val = 0; - status = m_impl->GetStckyFault_ForLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0; - - /* rev-limit-switch */ - val = 0; - status = m_impl->GetStckyFault_RevLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0; - - /* fwd-soft-limit */ - val = 0; - status = m_impl->GetStckyFault_ForSoftLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0; - - /* rev-soft-limit */ - val = 0; - status = m_impl->GetStckyFault_RevSoftLim(val); - if (status != CTR_OKAY) - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval |= (val) ? CANSpeedController::kRevSoftLimit : 0; - - return retval; -} - -void CANTalon::ClearStickyFaults() { - CTR_Code status = m_impl->ClearStickyFaults(); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); -} - -/** - * Set the maximum voltage change rate. This ramp rate is in affect regardless - * of which control mode the TALON is in. - * - * When in PercentVbus or Voltage output mode, the rate at which the voltage - * changes can be limited to reduce current spikes. Set this to 0.0 to disable - * rate limiting. - * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode - * in V/s. - */ -void CANTalon::SetVoltageRampRate(double rampRate) { - /* Caller is expressing ramp as Voltage per sec, assuming 12V is full. - Talon's throttle ramp is in dThrot/d10ms. 1023 is full fwd, -1023 is - full rev. */ - double rampRatedThrotPer10ms = (rampRate * 1023.0 / 12.0) / 100; - CTR_Code status = - m_impl->SetRampThrottle(static_cast(rampRatedThrotPer10ms)); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -void CANTalon::SetVoltageCompensationRampRate(double rampRate) { - /* when in voltage compensation mode, the voltage compensation rate - directly caps the change in target voltage */ - CTR_Code status = CTR_OKAY; - status = m_impl->SetVoltageCompensationRate(rampRate / 1000); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Sets a voltage change rate that applies only when a close loop contorl mode - * is enabled. - * This allows close loop specific ramp behavior. - * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode - * in V/s. - */ -void CANTalon::SetCloseLoopRampRate(double rampRate) { - CTR_Code status = m_impl->SetCloseLoopRampRate( - m_profile, rampRate * 1023.0 / 12.0 / 1000.0); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * @return The version of the firmware running on the Talon - */ -int CANTalon::GetFirmwareVersion() const { - int firmwareVersion; - CTR_Code status = m_impl->RequestParam(CanTalonSRX::eFirmVers); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - status = - m_impl->GetParamResponseInt32(CanTalonSRX::eFirmVers, firmwareVersion); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - - /* only sent once on boot */ - // CTR_Code status = m_impl->GetFirmVers(firmwareVersion); - // if (status != CTR_OKAY) { - // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - //} - - return firmwareVersion; -} - -/** - * @return The accumulator for I gain. - */ -int CANTalon::GetIaccum() const { - CTR_Code status = m_impl->RequestParam(CanTalonSRX::ePidIaccum); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - // small yield for getting response - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - int iaccum; - status = m_impl->GetParamResponseInt32(CanTalonSRX::ePidIaccum, iaccum); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return iaccum; -} - -/** - * Clear the accumulator for I gain. - */ -void CANTalon::ClearIaccum() { - CTR_Code status = m_impl->SetParam(CanTalonSRX::ePidIaccum, 0); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::ConfigNeutralMode(NeutralMode mode) { - CTR_Code status = CTR_OKAY; - switch (mode) { - default: - case kNeutralMode_Jumper: - // use default setting in flash based on webdash/BrakeCal button selection - status = m_impl->SetOverrideBrakeType( - CanTalonSRX::kBrakeOverride_UseDefaultsFromFlash); - break; - case kNeutralMode_Brake: - status = m_impl->SetOverrideBrakeType( - CanTalonSRX::kBrakeOverride_OverrideBrake); - break; - case kNeutralMode_Coast: - status = m_impl->SetOverrideBrakeType( - CanTalonSRX::kBrakeOverride_OverrideCoast); - break; - } - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * @return nonzero if brake is enabled during neutral. Zero if coast is enabled - * during neutral. - */ -int CANTalon::GetBrakeEnableDuringNeutral() const { - int brakeEn = 0; - CTR_Code status = m_impl->GetBrakeIsEnabled(brakeEn); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - return brakeEn; -} - -/** - * Configure how many codes per revolution are generated by your encoder. - * - * @param codesPerRev The number of counts per revolution. - */ -void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev) { - /* first save the scalar so that all getters/setter work as the user expects - */ - m_codesPerRev = codesPerRev; - /* next send the scalar to the Talon over CAN. This is so that the Talon can - * report it to whoever needs it, like the webdash. Don't bother checking - * the return, this is only for instrumentation and is not necessary for - * Talon functionality. - */ - (void)m_impl->SetParam(CanTalonSRX::eNumberEncoderCPR, m_codesPerRev); -} - -/** - * Configure the number of turns on the potentiometer. - * - * @param turns The number of turns of the potentiometer. - */ -void CANTalon::ConfigPotentiometerTurns(uint16_t turns) { - /* first save the scalar so that all getters/setter work as the user expects - */ - m_numPotTurns = turns; - /* next send the scalar to the Talon over CAN. This is so that the Talon can - * report it to whoever needs it, like the webdash. Don't bother checking - * the return, this is only for instrumentation and is not necessary for - * Talon functionality. - */ - (void)m_impl->SetParam(CanTalonSRX::eNumberPotTurns, m_numPotTurns); -} - -/** - * @deprecated not implemented - */ -void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition, - double reverseLimitPosition) { - ConfigLimitMode(kLimitMode_SoftPositionLimits); - ConfigForwardLimit(forwardLimitPosition); - ConfigReverseLimit(reverseLimitPosition); -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::DisableSoftPositionLimits() { - ConfigLimitMode(kLimitMode_SwitchInputsOnly); -} - -/** - * Overrides the forward and reverse limit switch enables. - * - * Unlike ConfigLimitMode, this function allows individual control of forward - * and reverse limit switch enables. - * Unlike ConfigLimitMode, this function does not affect the soft-limit features - * of Talon SRX. - * @see ConfigLimitMode() - */ -void CANTalon::ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, - bool bReverseLimitSwitchEn) { - CTR_Code status = CTR_OKAY; - int fwdRevEnable; - /* chose correct signal value based on caller's requests enables */ - if (!bForwardLimitSwitchEn) { - /* caller wants Forward Limit Switch OFF */ - if (!bReverseLimitSwitchEn) { - /* caller wants both OFF */ - fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev; - } else { - /* caller Forward OFF and Reverse ON */ - fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_EnableRev; - } - } else { - /* caller wants Forward Limit Switch ON */ - if (!bReverseLimitSwitchEn) { - /* caller wants Forward ON and Reverse OFF */ - fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_DisableRev; - } else { - /* caller wants both ON */ - fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev; - } - } - /* update signal and error check code */ - status = m_impl->SetOverrideLimitSwitchEn(fwdRevEnable); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Configures the soft limit enable (wear leveled persistent memory). - * Also sets the limit switch overrides. - */ -void CANTalon::ConfigLimitMode(LimitMode mode) { - CTR_Code status = CTR_OKAY; - switch (mode) { - case kLimitMode_SwitchInputsOnly: /** Only use switches for limits */ - /* turn OFF both limits. SRX has individual enables and polarity for each - * limit switch.*/ - status = m_impl->SetForwardSoftEnable(false); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - status = m_impl->SetReverseSoftEnable(false); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - /* override enable the limit switches, this circumvents the webdash */ - status = m_impl->SetOverrideLimitSwitchEn( - CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - break; - case kLimitMode_SoftPositionLimits: /** Use both switches and soft limits */ - /* turn on both limits. SRX has individual enables and polarity for each - * limit switch.*/ - status = m_impl->SetForwardSoftEnable(true); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - status = m_impl->SetReverseSoftEnable(true); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - /* override enable the limit switches, this circumvents the webdash */ - status = m_impl->SetOverrideLimitSwitchEn( - CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - break; - - case kLimitMode_SrxDisableSwitchInputs: - // disable both limit switches and soft limits - - /* turn on both limits. SRX has individual enables and polarity for each - * limit switch.*/ - status = m_impl->SetForwardSoftEnable(false); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - status = m_impl->SetReverseSoftEnable(false); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - /* override enable the limit switches, this circumvents the webdash */ - status = m_impl->SetOverrideLimitSwitchEn( - CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - break; - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::ConfigForwardLimit(double forwardLimitPosition) { - CTR_Code status = CTR_OKAY; - int nativeLimitPos = - ScaleRotationsToNativeUnits(m_feedbackDevice, forwardLimitPosition); - status = m_impl->SetForwardSoftLimit(nativeLimitPos); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Set the Forward Soft Limit Enable. - * - * This is the same setting that is in the Web-Based Configuration. - * - * @param bForwardSoftLimitEn true to enable Soft limit, false to disable. - */ -void CANTalon::ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn) { - CTR_Code status = CTR_OKAY; - status = m_impl->SetForwardSoftEnable(bForwardSoftLimitEn); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Set the Reverse Soft Limit Enable. - * - * This is the same setting that is in the Web-Based Configuration. - * - * @param bReverseSoftLimitEn true to enable Soft limit, false to disable. - */ -void CANTalon::ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn) { - CTR_Code status = CTR_OKAY; - status = m_impl->SetReverseSoftEnable(bReverseSoftLimitEn); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Change the fwd limit switch setting to normally open or closed. - * Talon will disable momentarilly if the Talon's current setting - * is dissimilar to the caller's requested setting. - * - * Since Talon saves setting to flash this should only affect - * a given Talon initially during robot install. - * - * @param normallyOpen true for normally open. false for normally closed. - */ -void CANTalon::ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen) { - CTR_Code status = - m_impl->SetParam(CanTalonSRX::eOnBoot_LimitSwitch_Forward_NormallyClosed, - normallyOpen ? 0 : 1); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Change the rev limit switch setting to normally open or closed. - * Talon will disable momentarilly if the Talon's current setting - * is dissimilar to the caller's requested setting. - * - * Since Talon saves setting to flash this should only affect - * a given Talon initially during robot install. - * - * @param normallyOpen true for normally open. false for normally closed. - */ -void CANTalon::ConfigRevLimitSwitchNormallyOpen(bool normallyOpen) { - CTR_Code status = - m_impl->SetParam(CanTalonSRX::eOnBoot_LimitSwitch_Reverse_NormallyClosed, - normallyOpen ? 0 : 1); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::ConfigReverseLimit(double reverseLimitPosition) { - CTR_Code status = CTR_OKAY; - int nativeLimitPos = - ScaleRotationsToNativeUnits(m_feedbackDevice, reverseLimitPosition); - status = m_impl->SetReverseSoftLimit(nativeLimitPos); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::ConfigMaxOutputVoltage(double voltage) { - /* config peak throttle when in closed-loop mode in the fwd and rev direction. - */ - ConfigPeakOutputVoltage(voltage, -voltage); -} - -void CANTalon::ConfigPeakOutputVoltage(double forwardVoltage, - double reverseVoltage) { - /* bounds checking */ - if (forwardVoltage > 12) - forwardVoltage = 12; - else if (forwardVoltage < 0) - forwardVoltage = 0; - if (reverseVoltage > 0) - reverseVoltage = 0; - else if (reverseVoltage < -12) - reverseVoltage = -12; - /* config calls */ - ConfigSetParameter(CanTalonSRX::ePeakPosOutput, 1023 * forwardVoltage / 12.0); - ConfigSetParameter(CanTalonSRX::ePeakNegOutput, 1023 * reverseVoltage / 12.0); -} - -void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage, - double reverseVoltage) { - /* bounds checking */ - if (forwardVoltage > 12) - forwardVoltage = 12; - else if (forwardVoltage < 0) - forwardVoltage = 0; - if (reverseVoltage > 0) - reverseVoltage = 0; - else if (reverseVoltage < -12) - reverseVoltage = -12; - /* config calls */ - ConfigSetParameter(CanTalonSRX::eNominalPosOutput, - 1023 * forwardVoltage / 12.0); - ConfigSetParameter(CanTalonSRX::eNominalNegOutput, - 1023 * reverseVoltage / 12.0); -} - -/** - * General set frame. Since the parameter is a general integral type, this can - * be used for testing future features. - */ -void CANTalon::ConfigSetParameter(int paramEnum, double value) { - CTR_Code status; - /* config peak throttle when in closed-loop mode in the positive direction. */ - status = m_impl->SetParam((CanTalonSRX::param_t)paramEnum, value); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * General get frame. Since the parameter is a general integral type, this can - * be used for testing future features. - */ -bool CANTalon::GetParameter(int paramEnum, double& dvalue) const { - bool retval = true; - /* send the request frame */ - CTR_Code status = m_impl->RequestParam((CanTalonSRX::param_t)paramEnum); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval = false; - } - /* small yield for getting response */ - std::this_thread::sleep_for( - std::chrono::microseconds(kDelayForSolicitedSignalsUs)); - /* get the last received update */ - status = m_impl->GetParamResponse((CanTalonSRX::param_t)paramEnum, dvalue); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - retval = false; - } - return retval; -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::ConfigFaultTime(float faultTime) { - /* SRX does not have fault time. SRX motor drive is only disabled for soft - * limits and limit-switch faults. */ - wpi_setWPIErrorWithContext(IncompatibleMode, "Fault Time not supported."); -} - -/** - * Fixup the sendMode so Set() serializes the correct demand value. - * Also fills the modeSelecet in the control frame to disabled. - * @param mode Control mode to ultimately enter once user calls Set(). - * @see Set() - */ -void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) { - m_controlMode = mode; - HAL_Report(HALUsageReporting::kResourceType_CANTalonSRX, m_deviceNumber + 1, - mode); - switch (mode) { - case kPercentVbus: - m_sendMode = kThrottle; - break; - case kCurrent: - m_sendMode = kCurrentMode; - break; - case kSpeed: - m_sendMode = kSpeedMode; - break; - case kPosition: - m_sendMode = kPositionMode; - break; - case kVoltage: - m_sendMode = kVoltageMode; - break; - case kFollower: - m_sendMode = kFollowerMode; - break; - case kMotionProfile: - m_sendMode = kMotionProfileMode; - break; - } - // Keep the talon disabled until Set() is called. - CTR_Code status = m_impl->SetModeSelect(static_cast(kDisabled)); - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) { - if (m_controlMode == mode) { - /* we already are in this mode, don't perform disable workaround */ - } else { - ApplyControlMode(mode); - } -} - -/** - * TODO documentation (see CANJaguar.cpp) - */ -CANSpeedController::ControlMode CANTalon::GetControlMode() const { - return m_controlMode; -} - -void CANTalon::SetExpiration(float timeout) { - m_safetyHelper->SetExpiration(timeout); -} - -float CANTalon::GetExpiration() const { - return m_safetyHelper->GetExpiration(); -} - -bool CANTalon::IsAlive() const { return m_safetyHelper->IsAlive(); } - -bool CANTalon::IsSafetyEnabled() const { - return m_safetyHelper->IsSafetyEnabled(); -} - -void CANTalon::SetSafetyEnabled(bool enabled) { - m_safetyHelper->SetSafetyEnabled(enabled); -} - -void CANTalon::GetDescription(std::ostringstream& desc) const { - desc << "CANTalon ID " << m_deviceNumber; -} - -/** - * @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon - * allows multiple sensors to be attached simultaneously, - * caller must specify which sensor to lookup. - * @return The number of native Talon units per rotation of the selected - * sensor. Zero if the necessary sensor information is not available. - * @see ConfigEncoderCodesPerRev - * @see ConfigPotentiometerTurns - */ -double CANTalon::GetNativeUnitsPerRotationScalar( - FeedbackDevice devToLookup) const { - bool scalingAvail = false; - CTR_Code status = CTR_OKAY; - double retval = 0; - switch (devToLookup) { - case QuadEncoder: { - /* When caller wants to lookup Quadrature, the QEI may be in 1x if the - * selected feedback is edge counter. Additionally if the quadrature - * source is the CTRE Mag encoder, then the CPR is known. This is nice in - * that the calling app does not require knowing the CPR at all. So do - * both checks here. - */ - int qeiPulsePerCount = 4; /* default to 4x */ - switch (m_feedbackDevice) { - case CtreMagEncoder_Relative: - case CtreMagEncoder_Absolute: - /* we assume the quadrature signal comes from the MagEnc, - of which we know the CPR already */ - retval = kNativePwdUnitsPerRotation; - scalingAvail = true; - break; - case EncRising: /* Talon's QEI is setup for 1x, so perform 1x math */ - case EncFalling: - qeiPulsePerCount = 1; - break; - case QuadEncoder: /* Talon's QEI is 4x */ - default: - // pulse width and everything else, assume its regular quad use. - break; - } - if (scalingAvail) { - /* already deduced the scalar above, we're done. */ - } else { - /* we couldn't deduce the scalar just based on the selection */ - if (0 == m_codesPerRev) { - /* caller has never set the CPR. Most likely caller - * is just using engineering units so fall to the - * bottom of this func. - */ - } else { - /* Talon expects PPR units */ - retval = qeiPulsePerCount * m_codesPerRev; - scalingAvail = true; - } - } - } break; - case EncRising: - case EncFalling: - if (0 == m_codesPerRev) { - /* caller has never set the CPR. Most likely caller - * is just using engineering units so fall to the - * bottom of this func. - */ - } else { - /* Talon expects PPR units */ - retval = 1 * m_codesPerRev; - scalingAvail = true; - } - break; - case AnalogPot: - case AnalogEncoder: - if (0 == m_numPotTurns) { - /* caller has never set the CPR. Most likely caller - * is just using engineering units so fall to the - * bottom of this func. - */ - } else { - retval = - static_cast(kNativeAdcUnitsPerRotation) / m_numPotTurns; - scalingAvail = true; - } - break; - case CtreMagEncoder_Relative: - case CtreMagEncoder_Absolute: - case PulseWidth: - retval = kNativePwdUnitsPerRotation; - scalingAvail = true; - break; - } - /* handle any detected errors */ - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } - /* if scaling information is not possible, signal caller - * by returning zero - */ - if (false == scalingAvail) retval = 0; - return retval; -} - -/** - * @param fullRotations double precision value representing number of - * rotations of selected feedback sensor. If user has - * never called the config routine for the selected - * sensor, then the caller is likely passing rotations - * in engineering units already, in which case it is - * returned as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return fullRotations in native engineering units of the Talon SRX firmware. - */ -int CANTalon::ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, - double fullRotations) const { - /* first assume we don't have config info, prep the default return */ - int retval = static_cast(fullRotations); - /* retrieve scaling info */ - double scalar = GetNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) retval = static_cast(fullRotations * scalar); - return retval; -} - -/** - * @param rpm double precision value representing number of rotations per - * minute of selected feedback sensor. If user has never called - * the config routine for the selected sensor, then the caller is - * likely passing rotations in engineering units already, in which - * case it is returned as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return sensor velocity in native engineering units of the Talon SRX - * firmware. - */ -int CANTalon::ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, - double rpm) const { - /* first assume we don't have config info, prep the default return */ - int retval = static_cast(rpm); - /* retrieve scaling info */ - double scalar = GetNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) { - retval = static_cast(rpm * kMinutesPer100msUnit * scalar); - } - return retval; -} - -/** - * @param nativePos integral position of the feedback sensor in native Talon - * SRX units. If user has never called the config routine for - * the selected sensor, then the return will be in TALON SRX - * units as well to match the behavior in the 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision number of rotations, unless config was never - * performed. - */ -double CANTalon::ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, - int nativePos) const { - /* first assume we don't have config info, prep the default return */ - double retval = static_cast(nativePos); - /* retrieve scaling info */ - double scalar = GetNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) retval = static_cast(nativePos) / scalar; - return retval; -} - -/** - * @param nativeVel integral velocity of the feedback sensor in native Talon - * SRX units. If user has never called the config routine for - * the selected sensor, then the return will be in TALON SRX - * units as well to match the behavior in the 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision of sensor velocity in RPM, unless config was never - * performed. - */ -double CANTalon::ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, - int nativeVel) const { - /* first assume we don't have config info, prep the default return */ - double retval = static_cast(nativeVel); - /* retrieve scaling info */ - double scalar = GetNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) - retval = static_cast(nativeVel) / (scalar * kMinutesPer100msUnit); - return retval; -} - -/** - * Enables Talon SRX to automatically zero the Sensor Position whenever an - * edge is detected on the index signal. - * - * @param enable boolean input, pass true to enable feature or false to - * disable. - * @param risingEdge boolean input, pass true to clear the position on rising - * edge, pass false to clear the position on falling edge. - */ -void CANTalon::EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge) { - if (enable) { - /* enable the feature, update the edge polarity first to ensure - it is correct before the feature is enabled. */ - ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity, risingEdge ? 1 : 0); - ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx, 1); - } else { - /* disable the feature first, then update the edge polarity. */ - ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx, 0); - ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity, risingEdge ? 1 : 0); - } -} - -/** - * 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 CANTalon::ChangeMotionControlFramePeriod(int periodMs) { - m_impl->ChangeMotionControlFramePeriod(periodMs); -} - -/** - * Clear the buffered motion profile in both Talon RAM (bottom), and in the API - * (top). - * - * Be sure to check GetMotionProfileStatus() to know when the buffer is actually - * cleared. - */ -void CANTalon::ClearMotionProfileTrajectories() { - m_impl->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. - */ -int CANTalon::GetMotionProfileTopLevelBufferCount() { - return m_impl->GetMotionProfileTopLevelBufferCount(); -} - -/** - * Push another trajectory point into the top level buffer (which is emptied - * into the Talon's bottom buffer as room allows). - * - * @param trajPt the trajectory point to insert into buffer. - * @return true if trajectory point push ok. CTR_BufferFull if buffer is full - * due to kMotionProfileTopBufferCapacity. - */ -bool CANTalon::PushMotionProfileTrajectory(const TrajectoryPoint& trajPt) { - /* convert positiona and velocity to native units */ - int targPos = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position); - int targVel = ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity); - /* bounds check signals that require it */ - int profileSlotSelect = (trajPt.profileSlotSelect) ? 1 : 0; - int timeDurMs = (trajPt.timeDurMs >= 255) - ? 255 - : trajPt.timeDurMs; /* cap time to 255ms */ - /* send it to the top level buffer */ - CTR_Code status = m_impl->PushMotionProfileTrajectory( - targPos, targVel, profileSlotSelect, timeDurMs, trajPt.velocityOnly, - trajPt.isLastPoint, trajPt.zeroPos); - return (status == CTR_OKAY) ? true : false; -} - -/** - * @return true if api-level (top) buffer is full. - */ -bool CANTalon::IsMotionProfileTopLevelBufferFull() { - return m_impl->IsMotionProfileTopLevelBufferFull(); -} - -/** - * 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 CANTalon::ProcessMotionProfileBuffer() { - m_impl->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] motionProfileStatus contains all progress information on the - * currently running MP. - */ -void CANTalon::GetMotionProfileStatus( - MotionProfileStatus& motionProfileStatus) { - uint32_t flags; - uint32_t profileSlotSelect; - int targPos, targVel; - uint32_t topBufferRem, topBufferCnt, btmBufferCnt; - uint32_t outputEnable; - /* retrieve all motion profile signals from status frame */ - CTR_Code status = m_impl->GetMotionProfileStatus( - flags, profileSlotSelect, targPos, targVel, topBufferRem, topBufferCnt, - btmBufferCnt, outputEnable); - /* completely update the caller's structure */ - motionProfileStatus.topBufferRem = topBufferRem; - motionProfileStatus.topBufferCnt = topBufferCnt; - motionProfileStatus.btmBufferCnt = btmBufferCnt; - motionProfileStatus.hasUnderrun = - (flags & CanTalonSRX::kMotionProfileFlag_HasUnderrun) ? true : false; - motionProfileStatus.isUnderrun = - (flags & CanTalonSRX::kMotionProfileFlag_IsUnderrun) ? true : false; - motionProfileStatus.activePointValid = - (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsValid) ? true : false; - motionProfileStatus.activePoint.isLastPoint = - (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsLast) ? true : false; - motionProfileStatus.activePoint.velocityOnly = - (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_VelOnly) ? true : false; - motionProfileStatus.activePoint.position = - ScaleNativeUnitsToRotations(m_feedbackDevice, targPos); - motionProfileStatus.activePoint.velocity = - ScaleNativeUnitsToRpm(m_feedbackDevice, targVel); - motionProfileStatus.activePoint.profileSlotSelect = profileSlotSelect; - switch (outputEnable) { - case CanTalonSRX::kMotionProf_Disabled: - motionProfileStatus.outputEnable = SetValueMotionProfileDisable; - break; - case CanTalonSRX::kMotionProf_Enable: - motionProfileStatus.outputEnable = SetValueMotionProfileEnable; - break; - case CanTalonSRX::kMotionProf_Hold: - motionProfileStatus.outputEnable = SetValueMotionProfileHold; - break; - default: - motionProfileStatus.outputEnable = SetValueMotionProfileDisable; - break; - } - motionProfileStatus.activePoint.zeroPos = - false; /* this signal is only used sending pts to Talon */ - motionProfileStatus.activePoint.timeDurMs = - 0; /* this signal is only used sending pts to Talon */ - - if (status != CTR_OKAY) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - } -} - -/** - * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is - * ready for another point, but the low level buffer is empty. - * - * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set - * until Robot Application clears it with this routine, which ensures Robot - * Application gets a chance to instrument or react. Caller could also check - * the isUnderrun flag which automatically clears when fault condition is - * removed. - */ -void CANTalon::ClearMotionProfileHasUnderrun() { - ConfigSetParameter(CanTalonSRX::eMotionProfileHasUnderrunErr, 0); -} - -/** - * Common interface for inverting direction of a speed controller. - * - * Only works in PercentVbus, speed, and Voltage modes. - * - * @param isInverted The state of inversion, true is inverted. - */ -void CANTalon::SetInverted(bool isInverted) { m_isInverted = isInverted; } - -/** - * Common interface for the inverting direction of a speed controller. - * - * @return isInverted The state of inversion, true is inverted. - */ -bool CANTalon::GetInverted() const { return m_isInverted; } - -/** - * Common interface for stopping the motor until the next Set() call. - * - * Part of the MotorSafety interface. - * - * @deprecated Call Disable instead. - */ -void CANTalon::StopMotor() { - Disable(); - m_stopped = true; -} - -void CANTalon::ValueChanged(ITable* source, llvm::StringRef key, - std::shared_ptr value, bool isNew) { - if (key == "Mode" && value->IsDouble()) - SetControlMode( - static_cast(value->GetDouble())); - if (key == "p" && value->IsDouble()) SetP(value->GetDouble()); - if (key == "i" && value->IsDouble()) SetI(value->GetDouble()); - if (key == "d" && value->IsDouble()) SetD(value->GetDouble()); - if (key == "f" && value->IsDouble()) SetF(value->GetDouble()); - if (key == "Enabled" && value->IsBoolean()) { - if (value->GetBoolean()) { - Enable(); - } else { - Disable(); - } - } - if (key == "Value" && value->IsDouble()) Set(value->GetDouble()); -} - -bool CANTalon::IsModePID(CANSpeedController::ControlMode mode) const { - return mode == kCurrent || mode == kSpeed || mode == kPosition; -} - -void CANTalon::UpdateTable() { - if (m_table != nullptr) { - m_table->PutString("~TYPE~", "CANSpeedController"); - m_table->PutString("Type", "CANTalon"); - m_table->PutNumber("Mode", m_controlMode); - m_table->PutNumber("p", GetP()); - m_table->PutNumber("i", GetI()); - m_table->PutNumber("d", GetD()); - m_table->PutNumber("f", GetF()); - m_table->PutBoolean("Enabled", IsControlEnabled()); - m_table->PutNumber("Value", Get()); - } -} - -void CANTalon::StartLiveWindowMode() { - if (m_table != nullptr) { - m_table->AddTableListener(this, true); - } -} - -void CANTalon::StopLiveWindowMode() { - if (m_table != nullptr) { - m_table->RemoveTableListener(this); - } -} - -std::string CANTalon::GetSmartDashboardType() const { - return "CANSpeedController"; -} - -void CANTalon::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); -} - -std::shared_ptr CANTalon::GetTable() const { return m_table; } diff --git a/wpilibcIntegrationTests/src/CANTalonTest.cpp b/wpilibcIntegrationTests/src/CANTalonTest.cpp deleted file mode 100644 index eeaa4389bd..0000000000 --- a/wpilibcIntegrationTests/src/CANTalonTest.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2014-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. */ -/*----------------------------------------------------------------------------*/ - -#include "CANTalon.h" // NOLINT(build/include_order) - -#include "TestBench.h" -#include "Timer.h" -#include "gtest/gtest.h" - -const int32_t deviceId = 0; - -TEST(CANTalonTest, QuickTest) { - double throttle = 0.1; - CANTalon talon(deviceId); - talon.SetControlMode(CANSpeedController::kPercentVbus); - talon.EnableControl(); - talon.Set(throttle); - Wait(0.25); - EXPECT_NEAR(talon.Get(), throttle, 5e-3); - talon.Set(-throttle); - Wait(0.25); - EXPECT_NEAR(talon.Get(), -throttle, 5e-3); - - talon.Disable(); - Wait(0.1); - EXPECT_FLOAT_EQ(talon.Get(), 0.0); -} - -TEST(CANTalonTest, SetGetPID) { - // Tests that we can actually set and get PID values as intended. - CANTalon talon(deviceId); - double p = 0.05, i = 0.098, d = 1.23; - talon.SetPID(p, i, d); - // Wait(0.03); - EXPECT_NEAR(p, talon.GetP(), 1e-5); - EXPECT_NEAR(i, talon.GetI(), 1e-5); - EXPECT_NEAR(d, talon.GetD(), 1e-5); - // Test with new values in case the talon was already set to the previous - // ones. - p = 0.15; - i = 0.198; - d = 1.03; - talon.SetPID(p, i, d); - // Wait(0.03); - EXPECT_NEAR(p, talon.GetP(), 1e-5); - EXPECT_NEAR(i, talon.GetI(), 1e-5); - EXPECT_NEAR(d, talon.GetD(), 1e-5); -} - -TEST(CANTalonTest, DISABLED_PositionModeWorks) { - CANTalon talon(deviceId); - talon.SetFeedbackDevice(CANTalon::AnalogPot); - talon.SetControlMode(CANSpeedController::kPosition); - Wait(0.1); - double p = 2; - double i = 0.00; - double d = 0.00; - Wait(0.2); - talon.SetControlMode(CANSpeedController::kPosition); - talon.SetFeedbackDevice(CANTalon::AnalogPot); - talon.SetPID(p, i, d); - Wait(0.2); - talon.Set(100); - Wait(100); - talon.Disable(); - EXPECT_NEAR(talon.Get(), 500, 1000); -} - -TEST(CANTalonTest, GetFaults) { - CANTalon talon(deviceId); - EXPECT_EQ(talon.GetFaults(), 0); - EXPECT_EQ(talon.GetStickyFaults(), 0); -} diff --git a/wpilibj/athena.gradle b/wpilibj/athena.gradle index fa979e46b7..ea0222aa92 100644 --- a/wpilibj/athena.gradle +++ b/wpilibj/athena.gradle @@ -163,7 +163,6 @@ task jniHeaders { args 'edu.wpi.first.wpilibj.hal.AccelerometerJNI' args 'edu.wpi.first.wpilibj.hal.AnalogJNI' args 'edu.wpi.first.wpilibj.hal.AnalogGyroJNI' - args 'edu.wpi.first.wpilibj.hal.CanTalonJNI' args 'edu.wpi.first.wpilibj.hal.ConstantsJNI' args 'edu.wpi.first.wpilibj.hal.CounterJNI' args 'edu.wpi.first.wpilibj.hal.DigitalGlitchFilterJNI' diff --git a/wpilibj/src/athena/cpp/lib/CanTalonJNI.cpp b/wpilibj/src/athena/cpp/lib/CanTalonJNI.cpp deleted file mode 100644 index 5773863ce4..0000000000 --- a/wpilibj/src/athena/cpp/lib/CanTalonJNI.cpp +++ /dev/null @@ -1,836 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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. */ -/*----------------------------------------------------------------------------*/ - -#include -#include - -#include "edu_wpi_first_wpilibj_hal_CanTalonJNI.h" - -#include "HAL/CanTalonSRX.h" - -#include "HALUtil.h" - -extern "C" { - -inline bool CheckCTRStatus(JNIEnv *env, CTR_Code status) { - if (status != CTR_OKAY) ReportError(env, (int32_t)status, false); - return status == CTR_OKAY; -} - -/* - * Class: edu_wpi_first_wpilibj_hal_CanTalonJNI - * Method: new_CanTalonSRX - * Signature: (III)J - */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX__III - (JNIEnv *env, jclass, jint deviceNumber, jint controlPeriodMs, jint enablePeriodMs) -{ - return (jlong)(new CanTalonSRX((int)deviceNumber, (int)controlPeriodMs, (int)enablePeriodMs)); -} - -/* - * Class: edu_wpi_first_wpilibj_hal_CanTalonJNI - * Method: new_CanTalonSRX - * Signature: (II)J - */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX__II - (JNIEnv *env, jclass, jint deviceNumber, jint controlPeriodMs) -{ - return (jlong)(new CanTalonSRX((int)deviceNumber, (int)controlPeriodMs)); -} - -/* - * Class: edu_wpi_first_wpilibj_hal_CanTalonJNI - * Method: new_CanTalonSRX - * Signature: (I)J - */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX__I - (JNIEnv *env, jclass, jint deviceNumber) -{ - return (jlong)(new CanTalonSRX((int)deviceNumber)); -} - -/* - * Class: edu_wpi_first_wpilibj_hal_CanTalonJNI - * Method: new_CanTalonSRX - * Signature: ()J - */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX__ - (JNIEnv *env, jclass) -{ - return (jlong)(new CanTalonSRX); -} - -/* - * Class: edu_wpi_first_wpilibj_hal_CanTalonJNI - * Method: delete_CanTalonSRX - * Signature: (J)V - */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_delete_1CanTalonSRX - (JNIEnv *env, jclass, jlong handle) -{ - delete (CanTalonSRX*)handle; -} - -/* - * Class: edu_wpi_first_wpilibj_hal_CanTalonJNI - * Method: GetMotionProfileStatus - * Signature: (JLedu/wpi/first/wpilibj/CANTalon;Ledu/wpi/first/wpilibj/CANTalon/MotionProfileStatus;)V - */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetMotionProfileStatus - (JNIEnv *env, jclass, jlong handle, jobject canTalon, jobject motionProfileStatus) -{ - static jmethodID setMotionProfileStatusFromJNI = nullptr; - if (!setMotionProfileStatusFromJNI) { - jclass cls = env->GetObjectClass(canTalon); - setMotionProfileStatusFromJNI = env->GetMethodID(cls, "setMotionProfileStatusFromJNI", "(Ledu/wpi/first/wpilibj/CANTalon$MotionProfileStatus;IIIIIIII)V"); - if (!setMotionProfileStatusFromJNI) return; - } - - uint32_t flags; - uint32_t profileSlotSelect; - int32_t targPos; - int32_t targVel; - uint32_t topBufferRem; - uint32_t topBufferCnt; - uint32_t btmBufferCnt; - uint32_t outputEnable; - CTR_Code status = ((CanTalonSRX*)handle)->GetMotionProfileStatus(flags, profileSlotSelect, targPos, targVel, topBufferRem, topBufferCnt, btmBufferCnt, outputEnable); - if (!CheckCTRStatus(env, status)) return; - - env->CallVoidMethod(canTalon, setMotionProfileStatusFromJNI, motionProfileStatus, (jint)flags, (jint)profileSlotSelect, (jint)targPos, (jint)targVel, (jint)topBufferRem, (jint)topBufferCnt, (jint)btmBufferCnt, (jint)outputEnable); -} - -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_Set - (JNIEnv *env, jclass, jlong handle, jdouble value) -{ - return ((CanTalonSRX*)handle)->Set((double)value); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetParam - (JNIEnv *env, jclass, jlong handle, jint paramEnum, jdouble value) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetParam((CanTalonSRX::param_t)paramEnum, (double)value); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_RequestParam - (JNIEnv *env, jclass, jlong handle, jint paramEnum) -{ - CTR_Code status = ((CanTalonSRX*)handle)->RequestParam((CanTalonSRX::param_t)paramEnum); - CheckCTRStatus(env, status); -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetParamResponse - (JNIEnv *env, jclass, jlong handle, jint paramEnum) -{ - double value; - CTR_Code status = ((CanTalonSRX*)handle)->GetParamResponse((CanTalonSRX::param_t)paramEnum, value); - CheckCTRStatus(env, status); - return value; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetParamResponseInt32 - (JNIEnv *env, jclass, jlong handle, jint paramEnum) -{ - int value; - CTR_Code status = ((CanTalonSRX*)handle)->GetParamResponseInt32((CanTalonSRX::param_t)paramEnum, value); - CheckCTRStatus(env, status); - return value; -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetPgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx, jdouble gain) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetPgain((unsigned)slotIdx, (double)gain); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetIgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx, jdouble gain) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetIgain((unsigned)slotIdx, (double)gain); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetDgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx, jdouble gain) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetDgain((unsigned)slotIdx, (double)gain); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetFgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx, jdouble gain) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetFgain((unsigned)slotIdx, (double)gain); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetIzone - (JNIEnv *env, jclass, jlong handle, jint slotIdx, jint zone) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetIzone((unsigned)slotIdx, (int)zone); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetCloseLoopRampRate - (JNIEnv *env, jclass, jlong handle, jint slotIdx, jint closeLoopRampRate) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetCloseLoopRampRate((unsigned)slotIdx, (int)closeLoopRampRate); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetVoltageCompensationRate - (JNIEnv *env, jclass, jlong handle, jdouble voltagePerMs) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetVoltageCompensationRate((double)voltagePerMs); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetSensorPosition - (JNIEnv *env, jclass, jlong handle, jint pos) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetSensorPosition((int)pos); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetForwardSoftLimit - (JNIEnv *env, jclass, jlong handle, jint forwardLimit) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetForwardSoftLimit((int)forwardLimit); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetReverseSoftLimit - (JNIEnv *env, jclass, jlong handle, jint reverseLimit) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetReverseSoftLimit((int)reverseLimit); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetForwardSoftEnable - (JNIEnv *env, jclass, jlong handle, jint enable) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetForwardSoftEnable((int)enable); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetReverseSoftEnable - (JNIEnv *env, jclass, jlong handle, jint enable) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetReverseSoftEnable((int)enable); - CheckCTRStatus(env, status); -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetPgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx) -{ - double gain; - CTR_Code status = ((CanTalonSRX*)handle)->GetPgain((unsigned)slotIdx, gain); - CheckCTRStatus(env, status); - return gain; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetIgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx) -{ - double gain; - CTR_Code status = ((CanTalonSRX*)handle)->GetIgain((unsigned)slotIdx, gain); - CheckCTRStatus(env, status); - return gain; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetDgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx) -{ - double gain; - CTR_Code status = ((CanTalonSRX*)handle)->GetDgain((unsigned)slotIdx, gain); - CheckCTRStatus(env, status); - return gain; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFgain - (JNIEnv *env, jclass, jlong handle, jint slotIdx) -{ - double gain; - CTR_Code status = ((CanTalonSRX*)handle)->GetFgain((unsigned)slotIdx, gain); - CheckCTRStatus(env, status); - return gain; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetIzone - (JNIEnv *env, jclass, jlong handle, jint slotIdx) -{ - int zone; - CTR_Code status = ((CanTalonSRX*)handle)->GetIzone((unsigned)slotIdx, zone); - CheckCTRStatus(env, status); - return zone; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetCloseLoopRampRate - (JNIEnv *env, jclass, jlong handle, jint slotIdx) -{ - int closeLoopRampRate; - CTR_Code status = ((CanTalonSRX*)handle)->GetCloseLoopRampRate((unsigned)slotIdx, closeLoopRampRate); - CheckCTRStatus(env, status); - return closeLoopRampRate; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetVoltageCompensationRate - (JNIEnv *env, jclass, jlong handle) -{ - double voltagePerMs; - CTR_Code status = ((CanTalonSRX*)handle)->GetVoltageCompensationRate(voltagePerMs); - CheckCTRStatus(env, status); - return voltagePerMs; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetForwardSoftLimit - (JNIEnv *env, jclass, jlong handle) -{ - int forwardLimit; - CTR_Code status = ((CanTalonSRX*)handle)->GetForwardSoftLimit(forwardLimit); - CheckCTRStatus(env, status); - return forwardLimit; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetReverseSoftLimit - (JNIEnv *env, jclass, jlong handle) -{ - int reverseLimit; - CTR_Code status = ((CanTalonSRX*)handle)->GetReverseSoftLimit(reverseLimit); - CheckCTRStatus(env, status); - return reverseLimit; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetForwardSoftEnable - (JNIEnv *env, jclass, jlong handle) -{ - int enable; - CTR_Code status = ((CanTalonSRX*)handle)->GetForwardSoftEnable(enable); - CheckCTRStatus(env, status); - return enable; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetReverseSoftEnable - (JNIEnv *env, jclass, jlong handle) -{ - int enable; - CTR_Code status = ((CanTalonSRX*)handle)->GetReverseSoftEnable(enable); - CheckCTRStatus(env, status); - return enable; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetPulseWidthRiseToFallUs - (JNIEnv *env, jclass, jlong handle) -{ - int param; - CTR_Code status = ((CanTalonSRX*)handle)->GetPulseWidthRiseToFallUs(param); - CheckCTRStatus(env, status); - return param; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_IsPulseWidthSensorPresent - (JNIEnv *env, jclass, jlong handle) -{ - int param; - CTR_Code status = ((CanTalonSRX*)handle)->IsPulseWidthSensorPresent(param); - CheckCTRStatus(env, status); - return param; -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetModeSelect2 - (JNIEnv *env, jclass, jlong handle, jint modeSelect, jint demand) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetModeSelect((int)modeSelect, (int)demand); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetStatusFrameRate - (JNIEnv *env, jclass, jlong handle, jint frameEnum, jint periodMs) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetStatusFrameRate((unsigned)frameEnum, (unsigned)periodMs); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_ClearStickyFaults - (JNIEnv *env, jclass, jlong handle) -{ - CTR_Code status = ((CanTalonSRX*)handle)->ClearStickyFaults(); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_ChangeMotionControlFramePeriod - (JNIEnv *env, jclass, jlong handle, jint periodMs) -{ - return ((CanTalonSRX*)handle)->ChangeMotionControlFramePeriod((uint32_t)periodMs); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_ClearMotionProfileTrajectories - (JNIEnv *env, jclass, jlong handle) -{ - return ((CanTalonSRX*)handle)->ClearMotionProfileTrajectories(); -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetMotionProfileTopLevelBufferCount - (JNIEnv *env, jclass, jlong handle) -{ - return ((CanTalonSRX*)handle)->GetMotionProfileTopLevelBufferCount(); -} -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_IsMotionProfileTopLevelBufferFull - (JNIEnv *env, jclass, jlong handle) -{ - return ((CanTalonSRX*)handle)->IsMotionProfileTopLevelBufferFull(); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_PushMotionProfileTrajectory - (JNIEnv *env, jclass, jlong handle, jint targPos, jint targVel, jint profileSlotSelect, jint timeDurMs, jint velOnly, jint isLastPoint, jint zeroPos) -{ - CTR_Code status = ((CanTalonSRX*)handle)->PushMotionProfileTrajectory((int)targPos, (int)targVel, (int)profileSlotSelect, (int)timeDurMs, (int)velOnly, (int)isLastPoint, (int)zeroPos); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_ProcessMotionProfileBuffer - (JNIEnv *env, jclass, jlong handle) -{ - return ((CanTalonSRX*)handle)->ProcessMotionProfileBuffer(); -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1OverTemp - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_OverTemp(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1UnderVoltage - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_UnderVoltage(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1ForLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_ForLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1RevLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_RevLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1HardwareFailure - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_HardwareFailure(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1ForSoftLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_ForSoftLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1RevSoftLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFault_RevSoftLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1OverTemp - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetStckyFault_OverTemp(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1UnderVoltage - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetStckyFault_UnderVoltage(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1ForLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetStckyFault_ForLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1RevLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetStckyFault_RevLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1ForSoftLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetStckyFault_ForSoftLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1RevSoftLim - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetStckyFault_RevSoftLim(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetAppliedThrottle - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetAppliedThrottle(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetCloseLoopErr - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetCloseLoopErr(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFeedbackDeviceSelect - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFeedbackDeviceSelect(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetModeSelect - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetModeSelect(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetLimitSwitchEn - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetLimitSwitchEn(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetLimitSwitchClosedFor - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetLimitSwitchClosedFor(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetLimitSwitchClosedRev - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetLimitSwitchClosedRev(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetSensorPosition - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetSensorPosition(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetSensorVelocity - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetSensorVelocity(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetCurrent - (JNIEnv * env, jclass, jlong handle) -{ - double retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetCurrent(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetBrakeIsEnabled - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetBrakeIsEnabled(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetEncPosition - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetEncPosition(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetEncVel - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetEncVel(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetEncIndexRiseEvents - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetEncIndexRiseEvents(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetQuadApin - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetQuadApin(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetQuadBpin - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetQuadBpin(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetQuadIdxpin - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetQuadIdxpin(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetAnalogInWithOv - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetAnalogInWithOv(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetAnalogInVel - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetAnalogInVel(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetTemp - (JNIEnv * env, jclass, jlong handle) -{ - double retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetTemp(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetBatteryV - (JNIEnv * env, jclass, jlong handle) -{ - double retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetBatteryV(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetResetCount - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetResetCount(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetResetFlags - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetResetFlags(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFirmVers - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetFirmVers(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetPulseWidthPosition - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetPulseWidthPosition(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetPulseWidthVelocity - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetPulseWidthVelocity(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetPulseWidthRiseToRiseUs - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetPulseWidthRiseToRiseUs(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1IsValid - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetActTraj_IsValid(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1ProfileSlotSelect - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetActTraj_ProfileSlotSelect(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1VelOnly - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetActTraj_VelOnly(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1IsLast - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetActTraj_IsLast(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetOutputType - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetOutputType(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetHasUnderrun - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetHasUnderrun(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetIsUnderrun - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetIsUnderrun(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetNextID - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetNextID(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetBufferIsFull - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetBufferIsFull(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetCount - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetCount(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1Velocity - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetActTraj_Velocity(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1Position - (JNIEnv * env, jclass, jlong handle) -{ - int retval; - CTR_Code status = ((CanTalonSRX*)handle)->GetActTraj_Position(retval); - CheckCTRStatus(env, status); - return retval; -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetDemand - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetDemand(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetOverrideLimitSwitchEn - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetOverrideLimitSwitchEn(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetFeedbackDeviceSelect - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetFeedbackDeviceSelect(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetRevMotDuringCloseLoopEn - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetRevMotDuringCloseLoopEn(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetOverrideBrakeType - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetOverrideBrakeType(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetModeSelect - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetModeSelect(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetProfileSlotSelect - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetProfileSlotSelect(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetRampThrottle - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetRampThrottle(param); - CheckCTRStatus(env, status); -} -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_SetRevFeedbackSensor - (JNIEnv * env, jclass, jlong handle, jint param) -{ - CTR_Code status = ((CanTalonSRX*)handle)->SetRevFeedbackSensor(param); - CheckCTRStatus(env, status); -} -} // extern "C" diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java deleted file mode 100644 index f7b035844c..0000000000 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java +++ /dev/null @@ -1,1865 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj; - -import edu.wpi.first.wpilibj.hal.CanTalonJNI; -import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; -import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.tables.ITable; -import edu.wpi.first.wpilibj.tables.ITableListener; - -public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedController { - private MotorSafetyHelper m_safetyHelper; - private boolean m_isInverted = false; - protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement; - /** - * Number of adc engineering units per 0 to 3.3V sweep. This is necessary for scaling Analog - * Position in rotations/RPM. - */ - private static final int kNativeAdcUnitsPerRotation = 1024; - /** - * Number of pulse width engineering units per full rotation. This is necessary for scaling Pulse - * Width Decoded Position in rotations/RPM. - */ - private static final double kNativePwdUnitsPerRotation = 4096.0; - /** - * Number of minutes per 100ms unit. Useful for scaling velocities measured by Talon's 100ms - * timebase to rotations per minute. - */ - private static final double kMinutesPer100msUnit = 1.0 / 600.0; - - public enum TalonControlMode implements CANSpeedController.ControlMode { - PercentVbus(0), Position(1), Speed(2), Current(3), Voltage(4), Follower(5), MotionProfile(6), - Disabled(15); - - @SuppressWarnings("MemberName") - public final int value; - - @SuppressWarnings("JavadocMethod") - public static TalonControlMode valueOf(int value) { - for (TalonControlMode mode : values()) { - if (mode.value == value) { - return mode; - } - } - - return null; - } - - TalonControlMode(int value) { - this.value = value; - } - - @Override - public boolean isPID() { - return this == Current || this == Speed || this == Position; - } - - @Override - public int getValue() { - return value; - } - } - - public enum FeedbackDevice { - QuadEncoder(0), AnalogPot(2), AnalogEncoder(3), EncRising(4), EncFalling(5), - CtreMagEncoder_Relative(6), CtreMagEncoder_Absolute(7), PulseWidth(8); - - @SuppressWarnings("MemberName") - public int value; - - @SuppressWarnings("JavadocMethod") - public static FeedbackDevice valueOf(int value) { - for (FeedbackDevice mode : values()) { - if (mode.value == value) { - return mode; - } - } - - return null; - } - - FeedbackDevice(int value) { - this.value = value; - } - } - - /** - * Depending on the sensor type, Talon can determine if sensor is plugged in ot not. - */ - public enum FeedbackDeviceStatus { - FeedbackStatusUnknown(0), FeedbackStatusPresent(1), FeedbackStatusNotPresent(2); - - @SuppressWarnings("MemberName") - public int value; - - @SuppressWarnings("JavadocMethod") - public static FeedbackDeviceStatus valueOf(int value) { - for (FeedbackDeviceStatus mode : values()) { - if (mode.value == value) { - return mode; - } - } - return null; - } - - FeedbackDeviceStatus(int value) { - this.value = value; - } - } - - /** - * Enumerated types for frame rate ms. - */ - public enum StatusFrameRate { - General(0), Feedback(1), QuadEncoder(2), AnalogTempVbat(3), PulseWidth(4); - - @SuppressWarnings("MemberName") - public int value; - - @SuppressWarnings("JavadocMethod") - public static StatusFrameRate valueOf(int value) { - for (StatusFrameRate mode : values()) { - if (mode.value == value) { - return mode; - } - } - return null; - } - - StatusFrameRate(int value) { - this.value = value; - } - } - - /** - * Enumerated types for Motion Control Set Values. When in Motion Profile control mode, these - * constants are paseed into set() to manipulate the motion profile executer. When changing modes, - * be sure to read the value back using getMotionProfileStatus() to ensure changes in output take - * effect before performing buffering actions. Disable will signal Talon to put motor output into - * neutral drive. Talon will stop processing motion profile points. This means the buffer is - * effectively disconnected from the executer, allowing the robot to gracefully clear and push new - * traj points. isUnderrun will get cleared. The active trajectory is also cleared. Enable will - * signal Talon to pop a trajectory point from it's buffer and process it. If the active - * trajectory is empty, Talon will shift in the next point. If the active traj is empty, and so is - * the buffer, the motor drive is neutral and isUnderrun is set. When active traj times out, and - * buffer has at least one point, Talon shifts in next one, and isUnderrun is cleared. When - * active traj times out, and buffer is empty, Talon keeps processing active traj and sets - * IsUnderrun. Hold will signal Talon keep processing the active trajectory indefinitely. If the - * active traj is cleared, Talon will neutral motor drive. Otherwise Talon will keep processing - * the active traj but it will not shift in points from the buffer. This means the buffer is - * effectively disconnected from the executer, allowing the robot to gracefully clear and push new - * traj points. isUnderrun is set if active traj is empty, otherwise it is cleared. isLast signal - * is also cleared. - * - *

Typical workflow: set(Disable), Confirm Disable takes effect, clear buffer and push buffer - * points, set(Enable) when enough points have been pushed to ensure no underruns, wait for MP to - * finish or decide abort, If MP finished gracefully set(Hold) to hold position servo and - * disconnect buffer, If MP is being aborted set(Disable) to neutral the motor and disconnect - * buffer, Confirm mode takes effect, clear buffer and push buffer points, and rinse-repeat. - */ - public enum SetValueMotionProfile { - Disable(0), Enable(1), Hold(2); - @SuppressWarnings("MemberName") - public int value; - - @SuppressWarnings("JavadocMethod") - public static SetValueMotionProfile valueOf(int value) { - for (SetValueMotionProfile mode : values()) { - if (mode.value == value) { - return mode; - } - } - return null; - } - - SetValueMotionProfile(int value) { - this.value = value; - } - } - - /** - * Motion Profile Trajectory Point This is simply a data transer object. - */ - @SuppressWarnings("MemberName") - public static class TrajectoryPoint { - public double position; //!< The position to servo to. - public double velocity; //!< The velocity to feed-forward. - /** - * Time in milliseconds to process this point. Value should be between 1ms and 255ms. If value - * is zero then Talon will default to 1ms. If value exceeds 255ms API will cap it. - */ - public int timeDurMs; - /** - * Which slot to get PIDF gains. PID is used for position servo. F is used as the Kv constant - * for velocity feed-forward. Typically this is hardcoded to the a particular slot, but you are - * free gain schedule if need be. - */ - public int profileSlotSelect; - /** - * Set to true to only perform the velocity feed-forward and not perform position servo. This is - * useful when learning how the position servo changes the motor response. The same could be - * accomplish by clearing the PID gains, however this is synchronous the streaming, and doesn't - * require restoing gains when finished. - * - *

Additionaly setting this basically gives you direct control of the motor output since - * motor output = targetVelocity X Kv, where Kv is our Fgain. This means you can also scheduling - * straight-throttle curves without relying on a sensor. - */ - public boolean velocityOnly; - /** - * Set to true to signal Talon that this is the final point, so do not attempt to pop another - * trajectory point from out of the Talon buffer. Instead continue processing this way point. - * Typically the velocity member variable should be zero so that the motor doesn't spin - * indefinitely. - */ - public boolean isLastPoint; - /** - * Set to true to signal Talon to zero the selected sensor. When generating MPs, one simple - * method is to make the first target position zero, and the final target position the target - * distance from the current position. Then when you fire the MP, the current position gets set - * to zero. If this is the intent, you can set zeroPos on the first trajectory point. - * - *

Otherwise you can leave this false for all points, and offset the positions of all - * trajectory points so they are correct. - */ - public boolean zeroPos; - } - - /** - * Motion Profile Status This is simply a data transer object. - */ - @SuppressWarnings("MemberName") - public static class MotionProfileStatus { - /** - * The available empty slots in the trajectory buffer. - * - *

The robot API holds a "top buffer" of trajectory points, so your applicaion can dump - * several points at once. The API will then stream them into the Talon's low-level buffer, - * allowing the Talon to act on them. - */ - public int topBufferRem; - /** - * The number of points in the top trajectory buffer. - */ - public int topBufferCnt; - /** - * The number of points in the low level Talon buffer. - */ - public int btmBufferCnt; - /** - * Set if isUnderrun ever gets set. Only is cleared by clearMotionProfileHasUnderrun() to ensure - * robot logic can react or instrument it. - * - * @see clearMotionProfileHasUnderrun() - */ - public boolean hasUnderrun; - /** - * This is set if Talon needs to shift a point from its buffer into the active trajectory point - * however the buffer is empty. This gets cleared automatically when is resolved. - */ - public boolean isUnderrun; - /** - * True if the active trajectory point has not empty, false otherwise. The members in - * activePoint are only valid if this signal is set. - */ - public boolean activePointValid; - /** - * The number of points in the low level Talon buffer. - */ - public TrajectoryPoint activePoint = new TrajectoryPoint(); - /** - * The current output mode of the motion profile executer (disabled, enabled, or hold). When - * changing the set() value in MP mode, it's important to check this signal to confirm the - * change takes effect before interacting with the top buffer. - */ - public SetValueMotionProfile outputEnable; - } - - private long m_handle; - private TalonControlMode m_controlMode; - private static double kDelayForSolicitedSignals = 0.004; - private double m_minimumInput; - private double m_maximumInput; - - private int m_deviceNumber; - private boolean m_controlEnabled; - private boolean m_stopped = false; - private int m_profile; - - double m_setPoint; - /** - * Encoder CPR, counts per rotations, also called codes per revoluion. Default value of zero means - * the API behaves as it did during the 2015 season, each position unit is a single pulse and - * there are four pulses per count (4X). Caller can use configEncoderCodesPerRev to set the - * quadrature encoder CPR. - */ - int m_codesPerRev; - /** - * Number of turns per rotation. For example, a 10-turn pot spins ten full rotations from a wiper - * voltage of zero to 3.3 volts. Therefore knowing the number of turns a full voltage sweep - * represents is necessary for calculating rotations and velocity. A default value of zero means - * the API behaves as it did during the 2015 season, there are 1024 position units from zero to - * 3.3V. - */ - int m_numPotTurns; - /** - * Although the Talon handles feedback selection, caching the feedback selection is helpful at the - * API level for scaling into rotations and RPM. - */ - FeedbackDevice m_feedbackDevice; - - /** - * Constructor for the CANTalon device. - * - * @param deviceNumber The CAN ID of the Talon SRX - */ - public CANTalon(int deviceNumber) { - m_deviceNumber = deviceNumber; - m_handle = CanTalonJNI.new_CanTalonSRX(deviceNumber); - m_safetyHelper = new MotorSafetyHelper(this); - m_controlEnabled = true; - m_profile = 0; - m_setPoint = 0; - m_codesPerRev = 0; - m_numPotTurns = 0; - m_feedbackDevice = FeedbackDevice.QuadEncoder; - setProfile(m_profile); - applyControlMode(TalonControlMode.PercentVbus); - LiveWindow.addActuator("CANTalon", m_deviceNumber, this); - } - - /** - * Constructor for the CANTalon device. - * - * @param deviceNumber The CAN ID of the Talon SRX - * @param controlPeriodMs The period in ms to send the CAN control frame. Period is bounded to - * [1ms,95ms]. - */ - public CANTalon(int deviceNumber, int controlPeriodMs) { - m_deviceNumber = deviceNumber; - /* bound period to be within [1 ms,95 ms] */ - m_handle = CanTalonJNI.new_CanTalonSRX(deviceNumber, controlPeriodMs); - m_safetyHelper = new MotorSafetyHelper(this); - m_controlEnabled = true; - m_profile = 0; - m_setPoint = 0; - m_codesPerRev = 0; - m_numPotTurns = 0; - m_feedbackDevice = FeedbackDevice.QuadEncoder; - setProfile(m_profile); - applyControlMode(TalonControlMode.PercentVbus); - LiveWindow.addActuator("CANTalon", m_deviceNumber, this); - } - - /** - * Constructor for the CANTalon device. - * - * @param deviceNumber The CAN ID of the Talon SRX - * @param controlPeriodMs The period in ms to send the CAN control frame. Period is bounded to - * [1ms,95ms]. - * @param enablePeriodMs The period in ms to send the enable control frame. Period is bounded to - * [1ms,95ms]. This typically is not required to specify, however this - * could be used to minimize the time between robot-enable and - * talon-motor-drive. - */ - public CANTalon(int deviceNumber, int controlPeriodMs, int enablePeriodMs) { - m_deviceNumber = deviceNumber; - m_handle = CanTalonJNI.new_CanTalonSRX(deviceNumber, controlPeriodMs, enablePeriodMs); - m_safetyHelper = new MotorSafetyHelper(this); - m_controlEnabled = true; - m_profile = 0; - m_setPoint = 0; - m_codesPerRev = 0; - m_numPotTurns = 0; - m_feedbackDevice = FeedbackDevice.QuadEncoder; - setProfile(m_profile); - applyControlMode(TalonControlMode.PercentVbus); - LiveWindow.addActuator("CANTalon", m_deviceNumber, this); - } - - @Override - public void pidWrite(double output) { - if (getControlMode() == TalonControlMode.PercentVbus) { - set(output); - } else { - throw new IllegalStateException("PID only supported in PercentVbus mode"); - } - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - m_pidSource = pidSource; - } - - @Override - public PIDSourceType getPIDSourceType() { - return m_pidSource; - } - - @Override - public double pidGet() { - return getPosition(); - } - - @SuppressWarnings("JavadocMethod") - public void delete() { - disable(); - if (m_handle != 0) { - CanTalonJNI.delete_CanTalonSRX(m_handle); - m_handle = 0; - } - } - - /** - * Sets the appropriate output on the talon, depending on the mode. - * - *

In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped. In Follower mode, - * the output is the integer device ID of the talon to duplicate. In Voltage mode, outputValue is - * in volts. In Current mode, outputValue is in amperes. In Speed mode, outputValue is in position - * change / 10ms. In Position mode, outputValue is in encoder ticks or an analog value, depending - * on the sensor. - * - * @param outputValue The setpoint value, as described above. - */ - public void set(double outputValue) { - /* feed safety helper since caller just updated our output */ - m_safetyHelper.feed(); - if (m_stopped) { - enableControl(); - m_stopped = false; - } - if (m_controlEnabled) { - m_setPoint = outputValue; /* cache set point for getSetpoint() */ - switch (m_controlMode) { - case PercentVbus: - CanTalonJNI.Set(m_handle, m_isInverted ? -outputValue : outputValue); - break; - case Follower: - CanTalonJNI.SetDemand(m_handle, (int) outputValue); - break; - case Voltage: - // Voltage is an 8.8 fixed point number. - int volts = (int) ((m_isInverted ? -outputValue : outputValue) * 256); - CanTalonJNI.SetDemand(m_handle, volts); - break; - case Speed: - CanTalonJNI.SetDemand(m_handle, scaleVelocityToNativeUnits(m_feedbackDevice, - (m_isInverted ? -outputValue : outputValue))); - break; - case Position: - CanTalonJNI.SetDemand(m_handle, scaleRotationsToNativeUnits(m_feedbackDevice, - outputValue)); - break; - case Current: - double milliamperes = (m_isInverted ? -outputValue : outputValue) * 1000.0; /* mA*/ - CanTalonJNI.SetDemand(m_handle, (int) milliamperes); - break; - case MotionProfile: - CanTalonJNI.SetDemand(m_handle, (int) outputValue); - break; - default: - break; - } - CanTalonJNI.SetModeSelect(m_handle, m_controlMode.value); - } - } - - /** - * Inverts the direction of the motor's rotation. Only works in PercentVbus mode. - * - * @param isInverted The state of inversion, true is inverted. - */ - @Override - public void setInverted(boolean isInverted) { - m_isInverted = isInverted; - } - - /** - * Common interface for the inverting direction of a speed controller. - * - * @return isInverted The state of inversion, true is inverted. - */ - @Override - public boolean getInverted() { - return m_isInverted; - } - - /** - * Resets the accumulated integral error and disables the controller. - * - *

The only difference between this and {@link PIDController#reset} is that the PIDController - * also resets the previous error for the D term, but the difference should have minimal effect as - * it will only last one cycle. - */ - public void reset() { - disable(); - clearIAccum(); - } - - /** - * Return true if Talon is enabled. - * - * @return true if the Talon is enabled and may be applying power to the motor - */ - public boolean isEnabled() { - return isControlEnabled(); - } - - /** - * Returns the difference between the setpoint and the current position. - * - * @return The error in units corresponding to whichever mode we are in. - * @see #set(double) set() for a detailed description of the various units. - */ - public double getError() { - return getClosedLoopError(); - } - - /** - * Calls {@link #set(double)}. - */ - public void setSetpoint(double setpoint) { - set(setpoint); - } - - /** - * Flips the sign (multiplies by negative one) the sensor values going into the talon. - * - *

This only affects position and velocity closed loop control. Allows for situations where you - * may have a sensor flipped and going in the wrong direction. - * - * @param flip True if sensor input should be flipped; False if not. - */ - public void reverseSensor(boolean flip) { - CanTalonJNI.SetRevFeedbackSensor(m_handle, flip ? 1 : 0); - } - - /** - * Flips the sign (multiplies by negative one) the throttle values going into the motor on the - * talon in closed loop modes. - * - * @param flip True if motor output should be flipped; False if not. - */ - public void reverseOutput(boolean flip) { - CanTalonJNI.SetRevMotDuringCloseLoopEn(m_handle, flip ? 1 : 0); - } - - /** - * Gets the current status of the Talon (usually a sensor value). - * - *

In Current mode: returns output current. In Speed mode: returns current speed. In Position - * mode: returns current sensor position. In PercentVbus and Follower modes: returns current - * applied throttle. - * - * @return The current sensor value of the Talon. - */ - public double get() { - switch (m_controlMode) { - case Voltage: - return getOutputVoltage(); - case Current: - return getOutputCurrent(); - case Speed: - return scaleNativeUnitsToRpm(m_feedbackDevice, CanTalonJNI.GetSensorVelocity(m_handle)); - case Position: - return scaleNativeUnitsToRotations(m_feedbackDevice, CanTalonJNI.GetSensorPosition( - m_handle)); - case PercentVbus: - default: - return (double) CanTalonJNI.GetAppliedThrottle(m_handle) / 1023.0; - } - } - - /** - * Get the current encoder position, regardless of whether it is the current feedback device. - * - * @return The current position of the encoder. - */ - public int getEncPosition() { - return CanTalonJNI.GetEncPosition(m_handle); - } - - public void setEncPosition(int newPosition) { - setParameter(CanTalonJNI.param_t.eEncPosition, newPosition); - } - - /** - * Get the current encoder velocity, regardless of whether it is the current feedback device. - * - * @return The current speed of the encoder. - */ - public int getEncVelocity() { - return CanTalonJNI.GetEncVel(m_handle); - } - - public int getPulseWidthPosition() { - return CanTalonJNI.GetPulseWidthPosition(m_handle); - } - - public void setPulseWidthPosition(int newPosition) { - setParameter(CanTalonJNI.param_t.ePwdPosition, newPosition); - } - - public int getPulseWidthVelocity() { - return CanTalonJNI.GetPulseWidthVelocity(m_handle); - } - - public int getPulseWidthRiseToFallUs() { - return CanTalonJNI.GetPulseWidthRiseToFallUs(m_handle); - } - - public int getPulseWidthRiseToRiseUs() { - return CanTalonJNI.GetPulseWidthRiseToRiseUs(m_handle); - } - - /** - * @param feedbackDevice which feedback sensor to check it if is connected. - * @return status of caller's specified sensor type. - */ - public FeedbackDeviceStatus isSensorPresent(FeedbackDevice feedbackDevice) { - FeedbackDeviceStatus retval = FeedbackDeviceStatus.FeedbackStatusUnknown; - /* detecting sensor health depends on which sensor caller cares about */ - switch (feedbackDevice) { - case QuadEncoder: - case AnalogPot: - case AnalogEncoder: - case EncRising: - case EncFalling: - /* no real good way to tell if these sensor - are actually present so return status unknown. */ - break; - case PulseWidth: - case CtreMagEncoder_Relative: - case CtreMagEncoder_Absolute: - /* all of these require pulse width signal to be present. */ - if (CanTalonJNI.IsPulseWidthSensorPresent(m_handle) == 0) { - /* Talon not getting a signal */ - retval = FeedbackDeviceStatus.FeedbackStatusNotPresent; - } else { - /* getting good signal */ - retval = FeedbackDeviceStatus.FeedbackStatusPresent; - } - break; - default: - break; - } - return retval; - } - - /** - * Get the number of of rising edges seen on the index pin. - * - * @return number of rising edges on idx pin. - */ - public int getNumberOfQuadIdxRises() { - return CanTalonJNI.GetEncIndexRiseEvents(m_handle); - } - - /** - * @return IO level of QUADA pin. - */ - public int getPinStateQuadA() { - return CanTalonJNI.GetQuadApin(m_handle); - } - - /** - * @return IO level of QUADB pin. - */ - public int getPinStateQuadB() { - return CanTalonJNI.GetQuadBpin(m_handle); - } - - /** - * @return IO level of QUAD Index pin. - */ - public int getPinStateQuadIdx() { - return CanTalonJNI.GetQuadIdxpin(m_handle); - } - - public void setAnalogPosition(int newPosition) { - setParameter(CanTalonJNI.param_t.eAinPosition, (double) newPosition); - } - - /** - * Get the current analog in position, regardless of whether it is the current feedback device. - * - *

The bottom ten bits is the ADC (0 - 1023) on the analog pin of the Talon. The upper 14 bits - * tracks the overflows and underflows (continuous sensor). - * - * @return The 24bit analog position. - */ - public int getAnalogInPosition() { - return CanTalonJNI.GetAnalogInWithOv(m_handle); - } - - /** - * Get the current analog in position, regardless of whether it is the current feedback device. - * - * @return The ADC (0 - 1023) on analog pin of the Talon. - */ - public int getAnalogInRaw() { - return getAnalogInPosition() & 0x3FF; - } - - /** - * Get the current encoder velocity, regardless of whether it is the current feedback device. - * - * @return The current speed of the analog in device. - */ - public int getAnalogInVelocity() { - return CanTalonJNI.GetAnalogInVel(m_handle); - } - - /** - * Get the current difference between the setpoint and the sensor value. - * - * @return The error, in whatever units are appropriate. - */ - public int getClosedLoopError() { - /* retrieve the closed loop error in native units */ - return CanTalonJNI.GetCloseLoopErr(m_handle); - } - - /** - * Set the allowable closed loop error. - * - * @param allowableCloseLoopError allowable closed loop error for selected profile. mA for Curent - * closed loop. Talon Native Units for position and velocity. - */ - public void setAllowableClosedLoopErr(int allowableCloseLoopError) { - if (m_profile == 0) { - setParameter(CanTalonJNI.param_t.eProfileParamSlot0_AllowableClosedLoopErr, (double) - allowableCloseLoopError); - } else { - setParameter(CanTalonJNI.param_t.eProfileParamSlot1_AllowableClosedLoopErr, (double) - allowableCloseLoopError); - } - } - - // Returns true if limit switch is closed. false if open. - public boolean isFwdLimitSwitchClosed() { - return (CanTalonJNI.GetLimitSwitchClosedFor(m_handle) == 0); - } - - // Returns true if limit switch is closed. false if open. - public boolean isRevLimitSwitchClosed() { - return (CanTalonJNI.GetLimitSwitchClosedRev(m_handle) == 0); - } - - // Returns true if break is enabled during neutral. false if coast. - public boolean getBrakeEnableDuringNeutral() { - return CanTalonJNI.GetBrakeIsEnabled(m_handle) != 0; - } - - /** - * Configure how many codes per revolution are generated by your encoder. - * - * @param codesPerRev The number of counts per revolution. - */ - public void configEncoderCodesPerRev(int codesPerRev) { - /* first save the scalar so that all getters/setter work as the user expects */ - m_codesPerRev = codesPerRev; - /* next send the scalar to the Talon over CAN. This is so that the Talon can report - it to whoever needs it, like the webdash. Don't bother checking the return, - this is only for instrumentation and is not necessary for Talon functionality. */ - setParameter(CanTalonJNI.param_t.eNumberEncoderCPR, m_codesPerRev); - } - - /** - * Configure the number of turns on the potentiometer. - * - * @param turns The number of turns of the potentiometer. - */ - public void configPotentiometerTurns(int turns) { - /* first save the scalar so that all getters/setter work as the user expects */ - m_numPotTurns = turns; - /* next send the scalar to the Talon over CAN. This is so that the Talon can report - it to whoever needs it, like the webdash. Don't bother checking the return, - this is only for instrumentation and is not necessary for Talon functionality. */ - setParameter(CanTalonJNI.param_t.eNumberPotTurns, m_numPotTurns); - } - - /** - * Returns temperature of Talon, in degrees Celsius. - */ - public double getTemperature() { - return CanTalonJNI.GetTemp(m_handle); - } - - /** - * Returns the current going through the Talon, in Amperes. - */ - public double getOutputCurrent() { - return CanTalonJNI.GetCurrent(m_handle); - } - - /** - * @return The voltage being output by the Talon, in Volts. - */ - public double getOutputVoltage() { - return getBusVoltage() * (double) CanTalonJNI.GetAppliedThrottle(m_handle) / 1023.0; - } - - /** - * @return The voltage at the battery terminals of the Talon, in Volts. - */ - public double getBusVoltage() { - return CanTalonJNI.GetBatteryV(m_handle); - } - - /** - * When using analog sensors, 0 units corresponds to 0V, 1023 units corresponds to 3.3V When using - * an analog encoder (wrapping around 1023 to 0 is possible) the units are still 3.3V per 1023 - * units. When using quadrature, each unit is a quadrature edge (4X) mode. - * - * @return The position of the sensor currently providing feedback. - */ - public double getPosition() { - return scaleNativeUnitsToRotations(m_feedbackDevice, CanTalonJNI.GetSensorPosition(m_handle)); - } - - public void setPosition(double pos) { - int nativePos = scaleRotationsToNativeUnits(m_feedbackDevice, pos); - CanTalonJNI.SetSensorPosition(m_handle, nativePos); - } - - /** - * The speed units will be in the sensor's native ticks per 100ms. - * - *

For analog sensors, 3.3V corresponds to 1023 units. So a speed of 200 equates to ~0.645 dV - * per 100ms or 6.451 dV per second. If this is an analog encoder, that likely means 1.9548 - * rotations per sec. For quadrature encoders, each unit corresponds a quadrature edge (4X). So a - * 250 count encoder will produce 1000 edge events per rotation. An example speed of 200 would - * then equate to 20% of a rotation per 100ms, or 10 rotations per second. - * - * @return The speed of the sensor currently providing feedback. - */ - public double getSpeed() { - return scaleNativeUnitsToRpm(m_feedbackDevice, CanTalonJNI.GetSensorVelocity(m_handle)); - } - - public TalonControlMode getControlMode() { - return m_controlMode; - } - - @SuppressWarnings("JavadocMethod") - public void setControlMode(int mode) { - TalonControlMode tcm = TalonControlMode.valueOf(mode); - if (tcm != null) { - changeControlMode(tcm); - } - } - - /** - * Fixup the m_controlMode so set() serializes the correct demand value. Also fills the - * modeSelecet in the control frame to disabled. - * - * @param controlMode Control mode to ultimately enter once user calls set(). - * @see #set - */ - private void applyControlMode(TalonControlMode controlMode) { - m_controlMode = controlMode; - if (controlMode == TalonControlMode.Disabled) { - m_controlEnabled = false; - } - // Disable until set() is called. - CanTalonJNI.SetModeSelect(m_handle, TalonControlMode.Disabled.value); - - HAL.report(tResourceType.kResourceType_CANTalonSRX, m_deviceNumber + 1, - controlMode.value); - } - - @SuppressWarnings("JavadocMethod") - public void changeControlMode(TalonControlMode controlMode) { - if (m_controlMode == controlMode) { - /* we already are in this mode, don't perform disable workaround */ - } else { - applyControlMode(controlMode); - } - } - - @SuppressWarnings("JavadocMethod") - public void setFeedbackDevice(FeedbackDevice device) { - /* save the selection so that future setters/getters know which scalars to apply */ - m_feedbackDevice = device; - /* pass feedback to actual CAN frame */ - CanTalonJNI.SetFeedbackDeviceSelect(m_handle, device.value); - } - - public void setStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs) { - CanTalonJNI.SetStatusFrameRate(m_handle, stateFrame.value, periodMs); - } - - public void enableControl() { - changeControlMode(m_controlMode); - m_controlEnabled = true; - } - - public void enable() { - enableControl(); - } - - public void disableControl() { - CanTalonJNI.SetModeSelect(m_handle, TalonControlMode.Disabled.value); - m_controlEnabled = false; - } - - public boolean isControlEnabled() { - return m_controlEnabled; - } - - /** - * Get the current proportional constant. - * - * @return double proportional constant for current profile. - */ - public double getP() { - // if(!(m_controlMode.equals(ControlMode.Position) || - // m_controlMode.equals(ControlMode.Speed))) { - // throw new - // IllegalStateException("PID mode only applies in Position and Speed modes."); - // } - - // Update the information that we have. - if (m_profile == 0) { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot0_P.value); - } else { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot1_P.value); - } - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetPgain(m_handle, m_profile); - } - - @SuppressWarnings("JavadocMethod") - public double getI() { - // if(!(m_controlMode.equals(ControlMode.Position) || - // m_controlMode.equals(ControlMode.Speed))) { - // throw new - // IllegalStateException("PID mode only applies in Position and Speed modes."); - // } - - // Update the information that we have. - if (m_profile == 0) { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot0_I.value); - } else { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot1_I.value); - } - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetIgain(m_handle, m_profile); - } - - @SuppressWarnings("JavadocMethod") - public double getD() { - // if(!(m_controlMode.equals(ControlMode.Position) || - // m_controlMode.equals(ControlMode.Speed))) { - // throw new - // IllegalStateException("PID mode only applies in Position and Speed modes."); - // } - - // Update the information that we have. - if (m_profile == 0) { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot0_D.value); - } else { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot1_D.value); - } - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetDgain(m_handle, m_profile); - } - - @SuppressWarnings("JavadocMethod") - public double getF() { - // if(!(m_controlMode.equals(ControlMode.Position) || - // m_controlMode.equals(ControlMode.Speed))) { - // throw new - // IllegalStateException("PID mode only applies in Position and Speed modes."); - // } - - // Update the information that we have. - if (m_profile == 0) { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot0_F.value); - } else { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot1_F.value); - } - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetFgain(m_handle, m_profile); - } - - @SuppressWarnings("JavadocMethod") - public double getIZone() { - // if(!(m_controlMode.equals(ControlMode.Position) || - // m_controlMode.equals(ControlMode.Speed))) { - // throw new - // IllegalStateException("PID mode only applies in Position and Speed modes."); - // } - - // Update the information that we have. - if (m_profile == 0) { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot0_IZone.value); - } else { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot1_IZone.value); - } - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetIzone(m_handle, m_profile); - } - - /** - * Get the closed loop ramp rate for the current profile. - * - *

Limits the rate at which the throttle will change. Only affects position and speed closed - * loop modes. - * - * @return rampRate Maximum change in voltage, in volts / sec. - * @see #setProfile For selecting a certain profile. - */ - public double getCloseLoopRampRate() { - // if(!(m_controlMode.equals(ControlMode.Position) || - // m_controlMode.equals(ControlMode.Speed))) { - // throw new - // IllegalStateException("PID mode only applies in Position and Speed modes."); - // } - - // Update the information that we have. - if (m_profile == 0) { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot0_CloseLoopRampRate - .value); - } else { - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eProfileParamSlot1_CloseLoopRampRate - .value); - } - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - double throttlePerMs = CanTalonJNI.GetCloseLoopRampRate(m_handle, m_profile); - return throttlePerMs / 1023.0 * 12.0 * 1000.0; - } - - /** - * Firmware version running on the Talon. - * - * @return The version of the firmware running on the Talon - */ - @SuppressWarnings("MethodName") - public long GetFirmwareVersion() { - - // Update the information that we have. - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.eFirmVers.value); - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetParamResponseInt32(m_handle, CanTalonJNI.param_t.eFirmVers.value); - } - - @SuppressWarnings({"MethodName", "JavadocMethod"}) - public long GetIaccum() { - - // Update the information that we have. - CanTalonJNI.RequestParam(m_handle, CanTalonJNI.param_t.ePidIaccum.value); - - // Briefly wait for new values from the Talon. - Timer.delay(kDelayForSolicitedSignals); - - return CanTalonJNI.GetParamResponseInt32(m_handle, CanTalonJNI.param_t.ePidIaccum.value); - } - - /** - * Set the proportional value of the currently selected profile. - * - * @param p Proportional constant for the currently selected PID profile. - * @see #setProfile For selecting a certain profile. - */ - @SuppressWarnings("ParameterName") - public void setP(double p) { - CanTalonJNI.SetPgain(m_handle, m_profile, p); - } - - /** - * Set the integration constant of the currently selected profile. - * - * @param i Integration constant for the currently selected PID profile. - * @see #setProfile For selecting a certain profile. - */ - @SuppressWarnings("ParameterName") - public void setI(double i) { - CanTalonJNI.SetIgain(m_handle, m_profile, i); - } - - /** - * Set the derivative constant of the currently selected profile. - * - * @param d Derivative constant for the currently selected PID profile. - * @see #setProfile For selecting a certain profile. - */ - @SuppressWarnings("ParameterName") - public void setD(double d) { - CanTalonJNI.SetDgain(m_handle, m_profile, d); - } - - /** - * Set the feedforward value of the currently selected profile. - * - * @param f Feedforward constant for the currently selected PID profile. - * @see #setProfile For selecting a certain profile. - */ - @SuppressWarnings("ParameterName") - public void setF(double f) { - CanTalonJNI.SetFgain(m_handle, m_profile, f); - } - - /** - * Set the integration zone of the current Closed Loop profile. - * - *

Whenever the error is larger than the izone value, the accumulated integration error is - * cleared so that high errors aren't racked up when at high errors. An izone value of 0 means no - * difference from a standard PIDF loop. - * - * @param izone Width of the integration zone. - * @see #setProfile For selecting a certain profile. - */ - public void setIZone(int izone) { - CanTalonJNI.SetIzone(m_handle, m_profile, izone); - } - - /** - * Set the closed loop ramp rate for the current profile. - * - *

Limits the rate at which the throttle will change. Only affects position and speed closed - * loop modes. - * - * @param rampRate Maximum change in voltage, in volts / sec. - * @see #setProfile For selecting a certain profile. - */ - public void setCloseLoopRampRate(double rampRate) { - // CanTalonJNI takes units of Throttle (0 - 1023) / 1ms. - int rate = (int) (rampRate * 1023.0 / 12.0 / 1000.0); - CanTalonJNI.SetCloseLoopRampRate(m_handle, m_profile, rate); - } - - /** - * Set the voltage ramp rate for the current profile. - * - *

Limits the rate at which the throttle will change. Affects all modes. - * - * @param rampRate Maximum change in voltage, in volts / sec. - */ - public void setVoltageRampRate(double rampRate) { - // CanTalonJNI takes units of Throttle (0 - 1023) / 10ms. - int rate = (int) (rampRate * 1023.0 / 12.0 / 100.0); - CanTalonJNI.SetRampThrottle(m_handle, rate); - } - - public void setVoltageCompensationRampRate(double rampRate) { - CanTalonJNI.SetVoltageCompensationRate(m_handle, rampRate / 1000); - } - - /** - * Clear the accumulator for I gain. - */ - @SuppressWarnings("MethodName") - public void ClearIaccum() { - CanTalonJNI.SetParam(m_handle, CanTalonJNI.param_t.ePidIaccum.value, 0); - } - - /** - * Sets control values for closed loop control. - * - * @param p Proportional constant. - * @param i Integration constant. - * @param d Differential constant. - * @param f Feedforward constant. - * @param izone Integration zone -- prevents accumulation of integration error with - * large errors. Setting this to zero will ignore any izone stuff. - * @param closeLoopRampRate Closed loop ramp rate. Maximum change in voltage, in volts / sec. - * @param profile which profile to set the pid constants for. You can have two profiles, - * with values of 0 or 1, allowing you to keep a second set of values on - * hand in the talon. In order to switch profiles without recalling - * setPID, you must call setProfile(). - */ - @SuppressWarnings("ParameterName") - public void setPID(double p, double i, double d, double f, int izone, double closeLoopRampRate, - int profile) { - if (profile != 0 && profile != 1) { - throw new IllegalArgumentException("Talon PID profile must be 0 or 1."); - } - m_profile = profile; - setProfile(profile); - setP(p); - setI(i); - setD(d); - setF(f); - setIZone(izone); - setCloseLoopRampRate(closeLoopRampRate); - } - - @SuppressWarnings("ParameterName") - public void setPID(double p, double i, double d) { - setPID(p, i, d, 0, 0, 0, m_profile); - } - - /** - * @return The latest value set using set(). - */ - public double getSetpoint() { - return m_setPoint; - } - - /** - * Select which closed loop profile to use, and uses whatever PIDF gains and the such that are - * already there. - */ - public void setProfile(int profile) { - if (profile != 0 && profile != 1) { - throw new IllegalArgumentException("Talon PID profile must be 0 or 1."); - } - m_profile = profile; - CanTalonJNI.SetProfileSlotSelect(m_handle, m_profile); - } - - /** - * Common interface for stopping a motor. - * - * @deprecated Use disableControl instead. - */ - @Override - @Deprecated - public void stopMotor() { - disableControl(); - m_stopped = true; - } - - @Override - public void disable() { - disableControl(); - } - - public int getDeviceID() { - return m_deviceNumber; - } - - // TODO: Documentation for all these accessors/setters for misc. stuff. - public void clearIAccum() { - CanTalonJNI.SetParam(m_handle, CanTalonJNI.param_t.ePidIaccum.value, 0); - } - - public void setForwardSoftLimit(double forwardLimit) { - int nativeLimitPos = scaleRotationsToNativeUnits(m_feedbackDevice, forwardLimit); - CanTalonJNI.SetForwardSoftLimit(m_handle, nativeLimitPos); - } - - public int getForwardSoftLimit() { - return CanTalonJNI.GetForwardSoftLimit(m_handle); - } - - public void enableForwardSoftLimit(boolean enable) { - CanTalonJNI.SetForwardSoftEnable(m_handle, enable ? 1 : 0); - } - - public boolean isForwardSoftLimitEnabled() { - return CanTalonJNI.GetForwardSoftEnable(m_handle) != 0; - } - - public void setReverseSoftLimit(double reverseLimit) { - int nativeLimitPos = scaleRotationsToNativeUnits(m_feedbackDevice, reverseLimit); - CanTalonJNI.SetReverseSoftLimit(m_handle, nativeLimitPos); - } - - public int getReverseSoftLimit() { - return CanTalonJNI.GetReverseSoftLimit(m_handle); - } - - public void enableReverseSoftLimit(boolean enable) { - CanTalonJNI.SetReverseSoftEnable(m_handle, enable ? 1 : 0); - } - - public boolean isReverseSoftLimitEnabled() { - return CanTalonJNI.GetReverseSoftEnable(m_handle) != 0; - } - - /** - * Configure the maximum voltage that the Jaguar will ever output. - * - *

This can be used to limit the maximum output voltage in all modes so that motors which - * cannot withstand full bus voltage can be used safely. - * - * @param voltage The maximum voltage output by the Jaguar. - */ - public void configMaxOutputVoltage(double voltage) { - /* config peak throttle when in closed-loop mode in the fwd and rev direction. */ - configPeakOutputVoltage(voltage, -voltage); - } - - @SuppressWarnings("JavadocMethod") - public void configPeakOutputVoltage(double forwardVoltage, double reverseVoltage) { - /* bounds checking */ - if (forwardVoltage > 12) { - forwardVoltage = 12; - } else if (forwardVoltage < 0) { - forwardVoltage = 0; - } - if (reverseVoltage > 0) { - reverseVoltage = 0; - } else if (reverseVoltage < -12) { - reverseVoltage = -12; - } - /* config calls */ - setParameter(CanTalonJNI.param_t.ePeakPosOutput, 1023 * forwardVoltage / 12.0); - setParameter(CanTalonJNI.param_t.ePeakNegOutput, 1023 * reverseVoltage / 12.0); - } - - @SuppressWarnings("JavadocMethod") - public void configNominalOutputVoltage(double forwardVoltage, double reverseVoltage) { - /* bounds checking */ - if (forwardVoltage > 12) { - forwardVoltage = 12; - } else if (forwardVoltage < 0) { - forwardVoltage = 0; - } - if (reverseVoltage > 0) { - reverseVoltage = 0; - } else if (reverseVoltage < -12) { - reverseVoltage = -12; - } - /* config calls */ - setParameter(CanTalonJNI.param_t.eNominalPosOutput, 1023 * forwardVoltage / 12.0); - setParameter(CanTalonJNI.param_t.eNominalNegOutput, 1023 * reverseVoltage / 12.0); - } - - /** - * General set frame. Since the parameter is a general integral type, this can be used for - * testing future features. - */ - public void setParameter(CanTalonJNI.param_t paramEnum, double value) { - CanTalonJNI.SetParam(m_handle, paramEnum.value, value); - } - - /** - * General get frame. Since the parameter is a general integral type, this can be used for - * testing future features. - */ - public double getParameter(CanTalonJNI.param_t paramEnum) { - /* transmit a request for this param */ - CanTalonJNI.RequestParam(m_handle, paramEnum.value); - /* Briefly wait for new values from the Talon. */ - Timer.delay(kDelayForSolicitedSignals); - /* poll out latest response value */ - return CanTalonJNI.GetParamResponse(m_handle, paramEnum.value); - } - - public void clearStickyFaults() { - CanTalonJNI.ClearStickyFaults(m_handle); - } - - public void enableLimitSwitch(boolean forward, boolean reverse) { - int mask = 4 + (forward ? 1 : 0) * 2 + (reverse ? 1 : 0); - CanTalonJNI.SetOverrideLimitSwitchEn(m_handle, mask); - } - - /** - * Configure the fwd limit switch to be normally open or normally closed. Talon will disable - * momentarilly if the Talon's current setting is dissimilar to the caller's requested setting. - * - *

Since Talon saves setting to flash this should only affect a given Talon initially during - * robot install. - * - * @param normallyOpen true for normally open. false for normally closed. - */ - @SuppressWarnings("MethodName") - public void ConfigFwdLimitSwitchNormallyOpen(boolean normallyOpen) { - CanTalonJNI.SetParam(m_handle, CanTalonJNI.param_t.eOnBoot_LimitSwitch_Forward_NormallyClosed - .value, - normallyOpen ? 0 : 1); - } - - /** - * Configure the rev limit switch to be normally open or normally closed. Talon will disable - * momentarilly if the Talon's current setting is dissimilar to the caller's requested setting. - * - *

Since Talon saves setting to flash this should only affect a given Talon initially during - * robot install. - * - * @param normallyOpen true for normally open. false for normally closed. - */ - @SuppressWarnings("MethodName") - public void ConfigRevLimitSwitchNormallyOpen(boolean normallyOpen) { - CanTalonJNI.SetParam(m_handle, CanTalonJNI.param_t.eOnBoot_LimitSwitch_Reverse_NormallyClosed - .value, - normallyOpen ? 0 : 1); - } - - public void enableBrakeMode(boolean brake) { - CanTalonJNI.SetOverrideBrakeType(m_handle, brake ? 2 : 1); - } - - public int getFaultOverTemp() { - return CanTalonJNI.GetFault_OverTemp(m_handle); - } - - public int getFaultUnderVoltage() { - return CanTalonJNI.GetFault_UnderVoltage(m_handle); - } - - public int getFaultForLim() { - return CanTalonJNI.GetFault_ForLim(m_handle); - } - - public int getFaultRevLim() { - return CanTalonJNI.GetFault_RevLim(m_handle); - } - - public int getFaultHardwareFailure() { - return CanTalonJNI.GetFault_HardwareFailure(m_handle); - } - - public int getFaultForSoftLim() { - return CanTalonJNI.GetFault_ForSoftLim(m_handle); - } - - public int getFaultRevSoftLim() { - return CanTalonJNI.GetFault_RevSoftLim(m_handle); - } - - public int getStickyFaultOverTemp() { - return CanTalonJNI.GetStckyFault_OverTemp(m_handle); - } - - public int getStickyFaultUnderVoltage() { - return CanTalonJNI.GetStckyFault_UnderVoltage(m_handle); - } - - public int getStickyFaultForLim() { - return CanTalonJNI.GetStckyFault_ForLim(m_handle); - } - - public int getStickyFaultRevLim() { - return CanTalonJNI.GetStckyFault_RevLim(m_handle); - } - - public int getStickyFaultForSoftLim() { - return CanTalonJNI.GetStckyFault_ForSoftLim(m_handle); - } - - public int getStickyFaultRevSoftLim() { - return CanTalonJNI.GetStckyFault_RevSoftLim(m_handle); - } - - /** - * Number of native units per rotation if scaling info is available. Zero if scaling information - * is not available. - * - * @return Number of native units per rotation. - */ - private double getNativeUnitsPerRotationScalar(FeedbackDevice devToLookup) { - double retval = 0; - boolean scalingAvail = false; - switch (devToLookup) { - case QuadEncoder: { - /* When caller wants to lookup Quadrature, the QEI may be in 1x if the selected feedback - * is edge counter. - * Additionally if the quadrature source is the CTRE Mag encoder, then the CPR is known. - * This is nice in that the calling app does not require knowing the CPR at all. - * So do both checks here. - */ - int qeiPulsePerCount = 4; /* default to 4x */ - switch (m_feedbackDevice) { - case CtreMagEncoder_Relative: - case CtreMagEncoder_Absolute: - /* we assume the quadrature signal comes from the MagEnc, - of which we know the CPR already */ - retval = kNativePwdUnitsPerRotation; - scalingAvail = true; - break; - case EncRising: /* Talon's QEI is setup for 1x, so perform 1x math */ - case EncFalling: - qeiPulsePerCount = 1; - break; - case QuadEncoder: /* Talon's QEI is 4x */ - default: /* pulse width and everything else, assume its regular quad use. */ - break; - } - if (scalingAvail) { - /* already deduced the scalar above, we're done. */ - } else { - /* we couldn't deduce the scalar just based on the selection */ - if (0 == m_codesPerRev) { - /* - * caller has never set the CPR. Most likely caller is just using engineering units so - * fall to the bottom of this func. - */ - } else { - /* Talon expects PPR units */ - retval = 4 * m_codesPerRev; - scalingAvail = true; - } - } - } - break; - case EncRising: - case EncFalling: - if (0 == m_codesPerRev) { - /* - * caller has never set the CPR. Most likely caller - * is just using engineering units so fall to the - * bottom of this func. - */ - } else { - /* Talon expects PPR units */ - retval = 1 * m_codesPerRev; - scalingAvail = true; - } - break; - case AnalogPot: - case AnalogEncoder: - if (0 == m_numPotTurns) { - /* - * caller has never set the CPR. Most likely caller - * is just using engineering units so fall to the - * bottom of this func. - */ - } else { - retval = (double) kNativeAdcUnitsPerRotation / m_numPotTurns; - scalingAvail = true; - } - break; - case CtreMagEncoder_Relative: - case CtreMagEncoder_Absolute: - case PulseWidth: - retval = kNativePwdUnitsPerRotation; - scalingAvail = true; - break; - default: - break; - } - /* if scaling info is not available give caller zero */ - if (false == scalingAvail) { - return 0; - } - return retval; - } - - /** - * @param fullRotations double precision value representing number of rotations of selected - * feedback sensor. If user has never called the config routine for the - * selected sensor, then the caller is likely passing rotations in - * engineering units already, in which case it is returned as is. - * @return fullRotations in native engineering units of the Talon SRX firmware. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev - */ - private int scaleRotationsToNativeUnits(FeedbackDevice devToLookup, double fullRotations) { - /* first assume we don't have config info, prep the default return */ - int retval = (int) fullRotations; - /* retrieve scaling info */ - double scalar = getNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) { - retval = (int) (fullRotations * scalar); - } - return retval; - } - - /** - * @param rpm double precision value representing number of rotations per minute of selected - * feedback sensor. If user has never called the config routine for the selected - * sensor, then the caller is likely passing rotations in engineering units already, in - * which case it is returned as is. - * @return sensor velocity in native engineering units of the Talon SRX firmware. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev - */ - private int scaleVelocityToNativeUnits(FeedbackDevice devToLookup, double rpm) { - /* first assume we don't have config info, prep the default return */ - int retval = (int) rpm; - /* retrieve scaling info */ - double scalar = getNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) { - retval = (int) (rpm * kMinutesPer100msUnit * scalar); - } - return retval; - } - - /** - * @param nativePos integral position of the feedback sensor in native Talon SRX units. If user - * has never called the config routine for the selected sensor, then the return - * will be in TALON SRX units as well to match the behavior in the 2015 season. - * @return double precision number of rotations, unless config was never performed. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev - */ - private double scaleNativeUnitsToRotations(FeedbackDevice devToLookup, int nativePos) { - /* first assume we don't have config info, prep the default return */ - double retval = (double) nativePos; - /* retrieve scaling info */ - double scalar = getNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) { - retval = ((double) nativePos) / scalar; - } - return retval; - } - - /** - * @param nativeVel integral velocity of the feedback sensor in native Talon SRX units. If user - * has never called the config routine for the selected sensor, then the return - * will be in TALON SRX units as well to match the behavior in the 2015 season. - * @return double precision of sensor velocity in RPM, unless config was never performed. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev - */ - private double scaleNativeUnitsToRpm(FeedbackDevice devToLookup, long nativeVel) { - /* first assume we don't have config info, prep the default return */ - double retval = (double) nativeVel; - /* retrieve scaling info */ - double scalar = getNativeUnitsPerRotationScalar(devToLookup); - /* apply scalar if its available */ - if (scalar > 0) { - retval = (double) (nativeVel) / (scalar * kMinutesPer100msUnit); - } - return retval; - } - - /** - * Enables Talon SRX to automatically zero the Sensor Position whenever an edge is detected on the - * index signal. - * - * @param enable boolean input, pass true to enable feature or false to disable. - * @param risingEdge boolean input, pass true to clear the position on rising edge, pass false to - * clear the position on falling edge. - */ - public void enableZeroSensorPositionOnIndex(boolean enable, boolean risingEdge) { - if (enable) { - /* enable the feature, update the edge polarity first to ensure - it is correct before the feature is enabled. */ - setParameter(CanTalonJNI.param_t.eQuadIdxPolarity, risingEdge ? 1 : 0); - setParameter(CanTalonJNI.param_t.eClearPositionOnIdx, 1); - } else { - /* disable the feature first, then update the edge polarity. */ - setParameter(CanTalonJNI.param_t.eClearPositionOnIdx, 0); - setParameter(CanTalonJNI.param_t.eQuadIdxPolarity, risingEdge ? 1 : 0); - } - } - - /** - * 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. - */ - public void changeMotionControlFramePeriod(int periodMs) { - CanTalonJNI.ChangeMotionControlFramePeriod(m_handle, periodMs); - } - - /** - * Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top). Be sure to - * check getMotionProfileStatus() to know when the buffer is actually cleared. - */ - public void clearMotionProfileTrajectories() { - CanTalonJNI.ClearMotionProfileTrajectories(m_handle); - } - - /** - * 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. - */ - public int getMotionProfileTopLevelBufferCount() { - return CanTalonJNI.GetMotionProfileTopLevelBufferCount(m_handle); - } - - /** - * Push another trajectory point into the top level buffer (which is emptied into the Talon's - * bottom buffer as room allows). - * - *

Will return CTR_OKAY if trajectory point push ok. CTR_BufferFull if buffer is full due to - * kMotionProfileTopBufferCapacity. - * - * @param trajPt {@link TrajectoryPoint} - * @return CTR_OKAY or CTR_BufferFull. - */ - public boolean pushMotionProfileTrajectory(TrajectoryPoint trajPt) { - /* check if there is room */ - if (isMotionProfileTopLevelBufferFull()) { - return false; - } - /* convert position and velocity to native units */ - int targPos = scaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position); - int targVel = scaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity); - /* bounds check signals that require it */ - int profileSlotSelect = (trajPt.profileSlotSelect > 0) ? 1 : 0; - int timeDurMs = trajPt.timeDurMs; - /* cap time to [0ms, 255ms], 0 and 1 are both interpreted as 1ms. */ - if (timeDurMs > 255) { - timeDurMs = 255; - } - if (timeDurMs < 0) { - timeDurMs = 0; - } - /* send it to the top level buffer */ - CanTalonJNI.PushMotionProfileTrajectory(m_handle, targPos, targVel, profileSlotSelect, - timeDurMs, trajPt.velocityOnly ? 1 : 0, trajPt.isLastPoint ? 1 : 0, trajPt.zeroPos ? 1 : 0); - return true; - } - - /** - * @return true if api-level (top) buffer is full. - */ - public boolean isMotionProfileTopLevelBufferFull() { - return CanTalonJNI.IsMotionProfileTopLevelBufferFull(m_handle); - } - - /** - * 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. - */ - public void processMotionProfileBuffer() { - CanTalonJNI.ProcessMotionProfileBuffer(m_handle); - } - - /** - * Retrieve all Motion Profile 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 motionProfileStatus [out] contains all progress information on the currently running MP. - * Caller should must instantiate the motionProfileStatus object first - * then pass into this routine to be filled. - */ - public void getMotionProfileStatus(MotionProfileStatus motionProfileStatus) { - CanTalonJNI.GetMotionProfileStatus(m_handle, this, motionProfileStatus); - } - - /** - * Internal method to set the contents. - */ - protected void setMotionProfileStatusFromJNI(MotionProfileStatus motionProfileStatus, - int flags, int profileSlotSelect, int targPos, - int targVel, int topBufferRem, int topBufferCnt, - int btmBufferCnt, int outputEnable) { - motionProfileStatus.topBufferRem = topBufferRem; - motionProfileStatus.topBufferCnt = topBufferCnt; - motionProfileStatus.btmBufferCnt = btmBufferCnt; - motionProfileStatus.hasUnderrun = ((flags & CanTalonJNI.kMotionProfileFlag_HasUnderrun) > 0); - motionProfileStatus.isUnderrun = ((flags & CanTalonJNI.kMotionProfileFlag_IsUnderrun) > 0); - motionProfileStatus.activePointValid = - ((flags & CanTalonJNI.kMotionProfileFlag_ActTraj_IsValid) > 0); - motionProfileStatus.activePoint.isLastPoint = - ((flags & CanTalonJNI.kMotionProfileFlag_ActTraj_IsLast) > 0); - motionProfileStatus.activePoint.velocityOnly = - ((flags & CanTalonJNI.kMotionProfileFlag_ActTraj_VelOnly) > 0); - motionProfileStatus.activePoint.position = - scaleNativeUnitsToRotations(m_feedbackDevice, targPos); - motionProfileStatus.activePoint.velocity = scaleNativeUnitsToRpm(m_feedbackDevice, targVel); - motionProfileStatus.activePoint.profileSlotSelect = profileSlotSelect; - motionProfileStatus.outputEnable = SetValueMotionProfile.valueOf(outputEnable); - motionProfileStatus.activePoint.zeroPos = false; // this signal is only used sending pts to - // Talon - motionProfileStatus.activePoint.timeDurMs = 0; // this signal is only used sending pts to - } - - /** - * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another - * point, but the low level buffer is empty. - * - *

Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until Robot - * Application clears it with this routine, which ensures Robot Application gets a chance to - * instrument or react. Caller could also check the isUnderrun flag which automatically clears - * when fault condition is removed. - */ - public void clearMotionProfileHasUnderrun() { - setParameter(CanTalonJNI.param_t.eMotionProfileHasUnderrunErr, 0); - } - - @Override - public void setExpiration(double timeout) { - m_safetyHelper.setExpiration(timeout); - } - - @Override - public double getExpiration() { - return m_safetyHelper.getExpiration(); - } - - @Override - public boolean isAlive() { - return m_safetyHelper.isAlive(); - } - - @Override - public boolean isSafetyEnabled() { - return m_safetyHelper.isSafetyEnabled(); - } - - @Override - public void setSafetyEnabled(boolean enabled) { - m_safetyHelper.setSafetyEnabled(enabled); - } - - @Override - public String getDescription() { - return "CANTalon ID " + m_deviceNumber; - } - - /* - * Live Window code, only does anything if live window is activated. - */ - - private ITable m_table = null; - private ITableListener m_tableListener = null; - - @Override - public void initTable(ITable subtable) { - m_table = subtable; - updateTable(); - } - - @Override - public void updateTable() { - CANSpeedController.super.updateTable(); - } - - @Override - public ITable getTable() { - return m_table; - } - - @Override - public void startLiveWindowMode() { - set(0); // Stop for safety - m_tableListener = createTableListener(); - m_table.addTableListener(m_tableListener, true); - } - - @Override - public void stopLiveWindowMode() { - set(0); // Stop for safety - // TODO: See if this is still broken - m_table.removeTableListener(m_tableListener); - } - -} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java deleted file mode 100644 index 6da630dc4a..0000000000 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java +++ /dev/null @@ -1,343 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.hal; - -@SuppressWarnings("MethodName") -public class CanTalonJNI extends JNIWrapper { - // Motion Profile status bits - public static final int kMotionProfileFlag_ActTraj_IsValid = 0x1; - public static final int kMotionProfileFlag_HasUnderrun = 0x2; - public static final int kMotionProfileFlag_IsUnderrun = 0x4; - public static final int kMotionProfileFlag_ActTraj_IsLast = 0x8; - public static final int kMotionProfileFlag_ActTraj_VelOnly = 0x10; - - /** - * 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. - */ - @SuppressWarnings("TypeName") - public 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); - - @SuppressWarnings("MemberName") - public final int value; - - param_t(int value) { - this.value = value; - } - } - - public static native long new_CanTalonSRX(int deviceNumber, int controlPeriodMs, - int enablePeriodMs); - - public static native long new_CanTalonSRX(int deviceNumber, int controlPeriodMs); - - public static native long new_CanTalonSRX(int deviceNumber); - - public static native long new_CanTalonSRX(); - - public static native void delete_CanTalonSRX(long handle); - - public static native void GetMotionProfileStatus(long handle, Object canTalon, - Object motionProfileStatus); - - public static native void Set(long handle, double value); - - public static native void SetParam(long handle, int paramEnum, double value); - - public static native void RequestParam(long handle, int paramEnum); - - public static native double GetParamResponse(long handle, int paramEnum); - - public static native int GetParamResponseInt32(long handle, int paramEnum); - - public static native void SetPgain(long handle, int slotIdx, double gain); - - public static native void SetIgain(long handle, int slotIdx, double gain); - - public static native void SetDgain(long handle, int slotIdx, double gain); - - public static native void SetFgain(long handle, int slotIdx, double gain); - - public static native void SetIzone(long handle, int slotIdx, int zone); - - public static native void SetCloseLoopRampRate(long handle, int slotIdx, int closeLoopRampRate); - - public static native void SetVoltageCompensationRate(long handle, double voltagePerMs); - - public static native void SetSensorPosition(long handle, int pos); - - public static native void SetForwardSoftLimit(long handle, int forwardLimit); - - public static native void SetReverseSoftLimit(long handle, int reverseLimit); - - public static native void SetForwardSoftEnable(long handle, int enable); - - public static native void SetReverseSoftEnable(long handle, int enable); - - public static native double GetPgain(long handle, int slotIdx); - - public static native double GetIgain(long handle, int slotIdx); - - public static native double GetDgain(long handle, int slotIdx); - - public static native double GetFgain(long handle, int slotIdx); - - public static native int GetIzone(long handle, int slotIdx); - - public static native int GetCloseLoopRampRate(long handle, int slotIdx); - - public static native double GetVoltageCompensationRate(long handle); - - public static native int GetForwardSoftLimit(long handle); - - public static native int GetReverseSoftLimit(long handle); - - public static native int GetForwardSoftEnable(long handle); - - public static native int GetReverseSoftEnable(long handle); - - public static native int GetPulseWidthRiseToFallUs(long handle); - - public static native int IsPulseWidthSensorPresent(long handle); - - public static native void SetModeSelect2(long handle, int modeSelect, int demand); - - public static native void SetStatusFrameRate(long handle, int frameEnum, int periodMs); - - public static native void ClearStickyFaults(long handle); - - public static native void ChangeMotionControlFramePeriod(long handle, int periodMs); - - public static native void ClearMotionProfileTrajectories(long handle); - - public static native int GetMotionProfileTopLevelBufferCount(long handle); - - public static native boolean IsMotionProfileTopLevelBufferFull(long handle); - - public static native void PushMotionProfileTrajectory(long handle, int targPos, int targVel, - int profileSlotSelect, int timeDurMs, - int velOnly, int isLastPoint, int zeroPos); - - public static native void ProcessMotionProfileBuffer(long handle); - - public static native int GetFault_OverTemp(long handle); - - public static native int GetFault_UnderVoltage(long handle); - - public static native int GetFault_ForLim(long handle); - - public static native int GetFault_RevLim(long handle); - - public static native int GetFault_HardwareFailure(long handle); - - public static native int GetFault_ForSoftLim(long handle); - - public static native int GetFault_RevSoftLim(long handle); - - public static native int GetStckyFault_OverTemp(long handle); - - public static native int GetStckyFault_UnderVoltage(long handle); - - public static native int GetStckyFault_ForLim(long handle); - - public static native int GetStckyFault_RevLim(long handle); - - public static native int GetStckyFault_ForSoftLim(long handle); - - public static native int GetStckyFault_RevSoftLim(long handle); - - public static native int GetAppliedThrottle(long handle); - - public static native int GetCloseLoopErr(long handle); - - public static native int GetFeedbackDeviceSelect(long handle); - - public static native int GetModeSelect(long handle); - - public static native int GetLimitSwitchEn(long handle); - - public static native int GetLimitSwitchClosedFor(long handle); - - public static native int GetLimitSwitchClosedRev(long handle); - - public static native int GetSensorPosition(long handle); - - public static native int GetSensorVelocity(long handle); - - public static native double GetCurrent(long handle); - - public static native int GetBrakeIsEnabled(long handle); - - public static native int GetEncPosition(long handle); - - public static native int GetEncVel(long handle); - - public static native int GetEncIndexRiseEvents(long handle); - - public static native int GetQuadApin(long handle); - - public static native int GetQuadBpin(long handle); - - public static native int GetQuadIdxpin(long handle); - - public static native int GetAnalogInWithOv(long handle); - - public static native int GetAnalogInVel(long handle); - - public static native double GetTemp(long handle); - - public static native double GetBatteryV(long handle); - - public static native int GetResetCount(long handle); - - public static native int GetResetFlags(long handle); - - public static native int GetFirmVers(long handle); - - public static native int GetPulseWidthPosition(long handle); - - public static native int GetPulseWidthVelocity(long handle); - - public static native int GetPulseWidthRiseToRiseUs(long handle); - - public static native int GetActTraj_IsValid(long handle); - - public static native int GetActTraj_ProfileSlotSelect(long handle); - - public static native int GetActTraj_VelOnly(long handle); - - public static native int GetActTraj_IsLast(long handle); - - public static native int GetOutputType(long handle); - - public static native int GetHasUnderrun(long handle); - - public static native int GetIsUnderrun(long handle); - - public static native int GetNextID(long handle); - - public static native int GetBufferIsFull(long handle); - - public static native int GetCount(long handle); - - public static native int GetActTraj_Velocity(long handle); - - public static native int GetActTraj_Position(long handle); - - public static native void SetDemand(long handle, int param); - - public static native void SetOverrideLimitSwitchEn(long handle, int param); - - public static native void SetFeedbackDeviceSelect(long handle, int param); - - public static native void SetRevMotDuringCloseLoopEn(long handle, int param); - - public static native void SetOverrideBrakeType(long handle, int param); - - public static native void SetModeSelect(long handle, int param); - - public static native void SetProfileSlotSelect(long handle, int param); - - public static native void SetRampThrottle(long handle, int param); - - public static native void SetRevFeedbackSensor(long handle, int param); -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java deleted file mode 100644 index 4bbd0719a6..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java +++ /dev/null @@ -1,156 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-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. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj; - -import org.junit.AfterClass; -import org.junit.Before; -import org.junit.BeforeClass; -import org.junit.Test; - -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.fixtures.SampleFixture; -import edu.wpi.first.wpilibj.test.AbstractComsSetup; - -import static org.junit.Assert.assertTrue; - -/** - * 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 m_fixture = new SampleFixture(); - - protected Logger getClassLogger() { - return logger; - } - - @BeforeClass - public static void classSetup() { - // Set up the fixture before the test is created - m_fixture.setup(); - } - - @Before - public void setUp() { - // Reset the fixture elements before every test - m_fixture.reset(); - } - - @AfterClass - public static void tearDown() { - // Clean up the fixture after the test - m_fixture.teardown(); - } - - private String errorMessage(double actual, double expected) { - String start = "Actual value was: "; - start += actual; - start += " Expected: "; - start += expected; - return start; - } - - /** - * Briefly run a CAN Talon and assert true. - */ - @Test - public void throttle() { - double throttle = 0.1; - CANTalon tal = new CANTalon(0); - tal.set(throttle); - Timer.delay(5.0); - assertTrue(errorMessage(tal.get(), throttle), Math.abs(throttle - tal.get()) < 1e-2); - - tal.set(-throttle); - Timer.delay(1.25); - assertTrue(errorMessage(tal.get(), -throttle), Math.abs(throttle + tal.get()) < 1e-2); - tal.reverseOutput(true); - tal.set(-throttle); - Timer.delay(1.25); - assertTrue(errorMessage(tal.get(), -throttle), Math.abs(throttle + tal.get()) < 1e-2); - - tal.disable(); - Timer.delay(0.2); - assertTrue(errorMessage(tal.get(), 0.0), Math.abs(tal.get()) < 1e-10); - } - - @Test - public void setGetPID() { - CANTalon talon = new CANTalon(0); - talon.changeControlMode(CANTalon.TalonControlMode.Position); - @SuppressWarnings({"LocalVariableName", "MultipleVariableDeclarations"}) - double p = 0.05, i = 0.098, d = 1.23; - talon.setPID(p, i, d); - assertTrue(errorMessage(talon.getP(), p), Math.abs(p - talon.getP()) < 1e-5); - assertTrue(errorMessage(talon.getI(), i), Math.abs(i - talon.getI()) < 1e-5); - assertTrue(errorMessage(talon.getD(), d), Math.abs(d - talon.getD()) < 1e-5); - // Test with new values in case the talon was already set to the previous - // ones. - p = 0.15; - i = 0.198; - d = 1.03; - talon.setPID(p, i, d); - assertTrue(errorMessage(talon.getP(), p), Math.abs(p - talon.getP()) < 1e-5); - assertTrue(errorMessage(talon.getI(), i), Math.abs(i - talon.getI()) < 1e-5); - assertTrue(errorMessage(talon.getD(), d), Math.abs(d - talon.getD()) < 1e-5); - } - - @Test - public void positionModeWorks() { - CANTalon talon = new CANTalon(0); - talon.changeControlMode(CANTalon.TalonControlMode.Position); - talon.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot); - Timer.delay(0.2); - @SuppressWarnings({"LocalVariableName", "MultipleVariableDeclarations"}) - double p = 1.0, i = 0.0, d = 0.00; - talon.setPID(p, i, d); - talon.set(100); - Timer.delay(5.0); - talon.reverseOutput(true); - Timer.delay(5.0); - // assertTrue(errorMessage(talon.get(), 100), Math.abs(100 - talon.get()) < - // 10); - assertTrue(true); - } - - @Test - public void testBrake() { - CANTalon talon = new CANTalon(0); - for (int i = 0; i < 5; i++) { - talon.enableBrakeMode(true); - Timer.delay(0.5); - talon.enableBrakeMode(false); - Timer.delay(0.5); - } - } - - //Test Get Fault methods (artf4814) - @Test - public void testGetFaults() { - CANTalon talon = new CANTalon(0); - talon.clearStickyFaults(); - - assertTrue(talon.getFaultOverTemp() == 0); - assertTrue(talon.getFaultUnderVoltage() == 0); - assertTrue(talon.getFaultForLim() == 0); - assertTrue(talon.getFaultRevLim() == 0); - assertTrue(talon.getFaultHardwareFailure() == 0); - assertTrue(talon.getFaultForSoftLim() == 0); - assertTrue(talon.getFaultRevSoftLim() == 0); - - assertTrue(talon.getStickyFaultOverTemp() == 0); - // assertTrue(talon.getStickyFaultUnderVoltage()==0); - assertTrue(talon.getStickyFaultForLim() == 0); - assertTrue(talon.getStickyFaultRevLim() == 0); - assertTrue(talon.getStickyFaultForSoftLim() == 0); - assertTrue(talon.getStickyFaultRevSoftLim() == 0); - } - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java index 3e54761bf2..38f84cf51b 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java @@ -58,10 +58,7 @@ public class SampleTest extends AbstractComsSetup { */ @Test public void test() { - CANTalon cantalon = new CANTalon(0); - cantalon.set(0.5); Timer.delay(0.5); - cantalon.set(0.0); assertTrue(true); } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index 1b0bda9891..3a7139d9a0 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; */ @RunWith(Suite.class) @SuiteClasses({AnalogCrossConnectTest.class, AnalogPotentiometerTest.class, - BuiltInAccelerometerTest.class, CANTalonTest.class, + BuiltInAccelerometerTest.class, CircularBufferTest.class, ConstantsPortsTest.class, CounterTest.class, DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, DriverStationTest.class, EncoderTest.class, FilterNoiseTest.class, FilterOutputTest.class, GyroTest.class,