From cd5765559af2f22287699d37086ffe35e360821f Mon Sep 17 00:00:00 2001 From: Omar Zrien Date: Mon, 30 Nov 2015 14:42:02 -0500 Subject: [PATCH] Last feature addition for CANTalon java/C++ user-facing API. - CANJaguar also touched up to report it can't do the new control mode (just like with follower). - New third optional param for talon c'tor to speed up enable control frame. - The pulse width routines have been moved to where the script generator puts them. No actual changes there but should help Peter integrate the latest code generator. Last feature additions for TalonSRX HAL for FRC2015-FRC-2016 season. -HAL driver uses control_5 frame if firmware supports it. This allows teams to see/confirm control settings taking effect before enabling the robot. For example selecting the sensor type and going to web-dash to check sensor values now works without having to enable the robot. -Motion profile HAL routines added. Tested on Single-Speed Double reduction (with slave Talon too). -Start moving ctre frame defs into a new common header (better then shoving a bunch of struct defs at top of module). -New child class in CANTalonSRX for buffering motion profile points. Not sure it would be best to leave it as is or make another module. It's trivial now so I thought that was acceptable, (in future it will likely possess compression strategies => no longer trivial). Change-Id: I803680c1a6669ca3f5157d7875942def6f75b540 --- hal/include/HAL/CanTalonSRX.h | 250 +++++- hal/include/ctre/CtreCanNode.h | 17 + hal/include/ctre/ctre.h | 13 +- hal/include/ctre/ctre_frames.h | 243 ++++++ hal/lib/Athena/ctre/CanTalonSRX.cpp | 777 ++++++++++++------ hal/lib/Athena/ctre/CtreCanNode.cpp | 71 +- wpilibc/Athena/include/CANSpeedController.h | 3 +- wpilibc/Athena/include/CANTalon.h | 273 +++++- wpilibc/Athena/src/CANJaguar.cpp | 5 +- wpilibc/Athena/src/CANTalon.cpp | 136 +++ wpilibj/src/athena/cpp/lib/CanTalonSRXJNI.cpp | 411 ++++++++- .../java/edu/wpi/first/wpilibj/CANTalon.java | 647 +++++++++++---- .../edu/wpi/first/wpilibj/CanTalonSRX.java | 73 +- .../wpi/first/wpilibj/hal/CanTalonJNI.java | 54 +- 14 files changed, 2462 insertions(+), 511 deletions(-) create mode 100644 hal/include/ctre/ctre_frames.h diff --git a/hal/include/HAL/CanTalonSRX.h b/hal/include/HAL/CanTalonSRX.h index 4cbff71eec..baac989887 100644 --- a/hal/include/HAL/CanTalonSRX.h +++ b/hal/include/HAL/CanTalonSRX.h @@ -71,11 +71,13 @@ */ #ifndef CanTalonSRX_H_ #define CanTalonSRX_H_ -#include "ctre/ctre.h" //BIT Defines + Typedefs +#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: @@ -100,6 +102,133 @@ private: sigs_t _sigs; //!< Catches signal updates that are solicited. Expect this to be very few. 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. @@ -115,7 +244,9 @@ private: CTR_Code GetParamResponseRaw(uint32_t paramEnum, int32_t & rawBits); public: static const int kDefaultControlPeriodMs = 10; //!< default control update rate is 10ms. - CanTalonSRX(int deviceNumber = 0,int controlPeriodMs = kDefaultControlPeriodMs); + static const int kDefaultEnablePeriodMs = 50; //!< default enable update rate is 50ms (when using the new control5 frame). + static const int kDefaultControl6PeriodMs = 10; //!< Default update rate for motion profile control 6. This only takes effect when calling uses MP functions. + CanTalonSRX(int deviceNumber = 0,int controlPeriodMs = kDefaultControlPeriodMs,int enablePeriodMs = kDefaultEnablePeriodMs); ~CanTalonSRX(); void Set(double value); /* mode select enumerations */ @@ -125,6 +256,7 @@ public: static const int kMode_CurrentCloseLoop = 3; //!< Current close loop - not done. static const int kMode_VoltCompen = 4; //!< Voltage Compensation Mode - not done. Demand is fixed pt target 8.8 volts. static const int kMode_SlaveFollower = 5; //!< Demand is the 6 bit Device ID of the 'master' TALON SRX. + static const int kMode_MotionProfile = 6; //!< Demand is '0' (Disabled), '1' (Enabled), or '2' (Hold). static const int kMode_NoDrive = 15; //!< Zero the output (honors brake/coast) regardless of demand. Might be useful if we need to change modes but can't atomically change all the signals we want in between. /* limit switch enumerations */ static const int kLimitSwitchOverride_UseDefaultsFromFlash = 1; @@ -152,6 +284,17 @@ public: 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 */ + static const int kMotionProf_Disabled = 0; //!< Motor output is neutral, Motion Profile Executer is not running. + static const int kMotionProf_Enable = 1; //!< Motor output is updated from Motion Profile Executer, MPE will process the buffered points. + static const int kMotionProf_Hold = 2; //!< Motor output is updated from Motion Profile Executer, MPE will stay processing current trajectory point. /** * Signal enumeration for generic signal access. * Although every signal is enumerated, only use this for traffic that must be solicited. @@ -247,6 +390,10 @@ public: eAinPosition=115, eProfileParamVcompRate=116, eProfileParamSlot1_AllowableClosedLoopErr=117, + eStatus9FrameRate=118, // TALON_Status_9_MotProfBuffer_100ms_t + eMotionProfileHasUnderrunErr = 119, + eReserved120 = 120, + eLegacyControlMode = 121, }param_t; /*---------------------setters and getters that use the solicated param request/response-------------*//** * Send a one shot frame to set an arbitrary signal. @@ -300,6 +447,85 @@ public: * 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 eventuall 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); @@ -339,6 +565,11 @@ public: 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 GetPulseWidthRiseToFallUs(int ¶m); + CTR_Code GetPulseWidthRiseToRiseUs(int ¶m); + CTR_Code IsPulseWidthSensorPresent(int ¶m); CTR_Code SetDemand(int param); CTR_Code SetOverrideLimitSwitchEn(int param); CTR_Code SetFeedbackDeviceSelect(int param); @@ -349,11 +580,6 @@ public: CTR_Code SetProfileSlotSelect(int param); CTR_Code SetRampThrottle(int param); CTR_Code SetRevFeedbackSensor(int param); - CTR_Code GetPulseWidthPosition(int ¶m); - CTR_Code GetPulseWidthVelocity(int ¶m); - CTR_Code GetPulseWidthRiseToFallUs(int ¶m); - CTR_Code GetPulseWidthRiseToRiseUs(int ¶m); - CTR_Code IsPulseWidthSensorPresent(int ¶m); }; extern "C" { void *c_TalonSRX_Create(int deviceNumber, int controlPeriodMs); @@ -401,6 +627,11 @@ extern "C" { 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_GetPulseWidthRiseToFallUs(void *handle, int *param); + CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param); + CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(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); @@ -411,11 +642,6 @@ extern "C" { CTR_Code c_TalonSRX_SetProfileSlotSelect(void *handle, int param); CTR_Code c_TalonSRX_SetRampThrottle(void *handle, int param); CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param); - CTR_Code c_TalonSRX_GetPulseWidthPosition(void *handle, int *param); - CTR_Code c_TalonSRX_GetPulseWidthVelocity(void *handle, int *param); - CTR_Code c_TalonSRX_GetPulseWidthRiseToFallUs(void *handle, int *param); - CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param); - CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param); } #endif diff --git a/hal/include/ctre/CtreCanNode.h b/hal/include/ctre/CtreCanNode.h index 4af7307c4b..270759866c 100644 --- a/hal/include/ctre/CtreCanNode.h +++ b/hal/include/ctre/CtreCanNode.h @@ -52,10 +52,26 @@ protected: }; UINT8 _deviceNumber; void RegisterRx(uint32_t arbId); + /** + * Schedule a CAN Frame for periodic transmit. Assume eight byte DLC and zero value for initial transmission. + * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids. + * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit. + */ void RegisterTx(uint32_t arbId, uint32_t periodMs); + /** + * Schedule a CAN Frame for periodic transmit. + * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids. + * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit. + * @param dlc Number of bytes to transmit (0 to 8). + * @param initialFrame Ptr to the frame data to schedule for transmitting. Passing null will result + * in defaulting to zero data value. + */ + void RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame); + void UnregisterTx(uint32_t arbId); CTR_Code GetRx(uint32_t arbId,uint8_t * dataBytes,uint32_t timeoutMs); void FlushTx(uint32_t arbId); + bool ChangeTxPeriod(uint32_t arbId, uint32_t periodMs); template txTask GetTx(uint32_t arbId) { @@ -86,6 +102,7 @@ private: uint32_t arbId; uint8_t toSend[8]; uint32_t periodMs; + uint8_t dlc; }; class rxEvent_t{ diff --git a/hal/include/ctre/ctre.h b/hal/include/ctre/ctre.h index c2d3f69614..a0f99b3071 100644 --- a/hal/include/ctre/ctre.h +++ b/hal/include/ctre/ctre.h @@ -1,5 +1,9 @@ -#ifndef GLOBAL_H -#define GLOBAL_H +/** + * @file ctre.h + * Common header for all CTRE HAL modules. + */ +#ifndef CTRE_H +#define CTRE_H //Bit Defines #define BIT0 0x01 @@ -45,6 +49,9 @@ typedef enum { CTR_UnexpectedArbId, //!< Specified CAN Id is invalid. CTR_TxFailed, //!< Could not transmit the CAN frame. CTR_SigNotUpdated, //!< Have not received an value response for signal. + CTR_BufferFull, //!< Caller attempted to insert data into a buffer that is full. }CTR_Code; -#endif +#include "ctre_frames.h" + +#endif /* CTRE_H */ diff --git a/hal/include/ctre/ctre_frames.h b/hal/include/ctre/ctre_frames.h new file mode 100644 index 0000000000..f131538fa9 --- /dev/null +++ b/hal/include/ctre/ctre_frames.h @@ -0,0 +1,243 @@ +/** + * @file ctre_frames.h + * CAN Encoder/Decoder Structures for CTRE devices. + */ +#ifndef CTRE_FRAMES_H +#define CTRE_FRAMES_H + +/** control */ +typedef struct _TALON_Control_1_General_10ms_t { + unsigned TokenH:8; + unsigned TokenL:8; + unsigned DemandH:8; + unsigned DemandM:8; + unsigned DemandL:8; + unsigned ProfileSlotSelect:1; + unsigned FeedbackDeviceSelect:4; + unsigned OverrideLimitSwitchEn:3; + unsigned RevFeedbackSensor:1; + unsigned RevMotDuringCloseLoopEn:1; + unsigned OverrideBrakeType:2; + unsigned ModeSelect:4; + unsigned RampThrottle:8; +} TALON_Control_1_General_10ms_t ; + +/* TALON_Control_2_Rates_OneShot_t removed since it has been deprecated */ + +typedef struct _TALON_Control_3_ClearFlags_OneShot_t { + unsigned ZeroFeedbackSensor:1; + unsigned ClearStickyFaults:1; +} TALON_Control_3_ClearFlags_OneShot_t ; + +typedef struct _TALON_Control_5_General_10ms_t { + unsigned ThrottleBump_h3:3; + unsigned ReservedZero:5; + unsigned ThrottleBump_l8:8; + unsigned DemandH:8; + unsigned DemandM:8; + unsigned DemandL:8; + unsigned ProfileSlotSelect:1; + unsigned FeedbackDeviceSelect:4; + unsigned OverrideLimitSwitchEn:3; + unsigned RevFeedbackSensor:1; + unsigned RevMotDuringCloseLoopEn:1; + unsigned OverrideBrakeType:2; + unsigned ModeSelect:4; + unsigned RampThrottle:8; +} TALON_Control_5_General_10ms_t ; + +typedef struct _TALON_Control_6_MotProfAddTrajPoint_t { + unsigned huffCode:2; //!< Compression coding + unsigned NextPt_VelOnly:1; + unsigned NextPt_IsLast:1; + unsigned reserved0:2; + unsigned NextPt_ZeroPosition:1; + unsigned NextPt_ProfileSlotSelect:1; + unsigned Idx:4; + unsigned reserved1:4; + unsigned restOfFrame0:8; + unsigned restOfFrame1:8; + unsigned restOfFrame2:8; + unsigned restOfFrame3:8; + unsigned restOfFrame4:8; + unsigned restOfFrame5:8; +} TALON_Control_6_MotProfAddTrajPoint_t; + +typedef struct _TALON_Control_6_MotProfAddTrajPoint_huff0_t { + unsigned huffCode_expect_0:2; //!< Compression coding + unsigned NextPt_VelOnly:1; + unsigned NextPt_IsLast:1; + unsigned reserved0:2; + unsigned NextPt_ZeroPosition:1; + unsigned NextPt_ProfileSlotSelect:1; + unsigned Idx:4; + unsigned reserved1:4; + unsigned NextPt_DurationMs:8; + unsigned NextPt_VelocityH:8; + unsigned NextPt_VelocityL:8; + unsigned NextPt_PositionH:8; + unsigned NextPt_PositionM:8; + unsigned NextPt_PositionL:8; +} TALON_Control_6_MotProfAddTrajPoint_huff0_t; + +typedef struct _TALON_Control_6_MotProfAddTrajPoint_huff1_t { + unsigned huffCode_expect_1:2; //!< Compression coding + unsigned NextPt_VelOnly:1; + unsigned NextPt_IsLast:1; + unsigned reserved0:2; + unsigned NextPt_ZeroPosition:1; + unsigned NextPt_ProfileSlotSelect:1; + unsigned Idx:4; + unsigned reserved1:4; + unsigned NextPt_DurationMs:8; + unsigned NextPt_SameVelocityH:8; + unsigned NextPt_SameVelocityL:8; + unsigned NextPt_DeltaPositionH:8; + unsigned NextPt_DeltaPositionL:8; + unsigned NextPt_Count:8; +} TALON_Control_6_MotProfAddTrajPoint_huff1_t; + +/** status */ +typedef struct _TALON_Status_1_General_10ms_t { + unsigned CloseLoopErrH:8; + unsigned CloseLoopErrM:8; + unsigned CloseLoopErrL:8; + unsigned AppliedThrottle_h3:3; + unsigned Fault_RevSoftLim:1; + unsigned Fault_ForSoftLim:1; + unsigned TokLocked:1; + unsigned LimitSwitchClosedRev:1; + unsigned LimitSwitchClosedFor:1; + unsigned AppliedThrottle_l8:8; + unsigned ModeSelect_h1:1; + unsigned FeedbackDeviceSelect:4; + unsigned LimitSwitchEn:3; + unsigned Fault_HardwareFailure:1; + unsigned Fault_RevLim:1; + unsigned Fault_ForLim:1; + unsigned Fault_UnderVoltage:1; + unsigned Fault_OverTemp:1; + unsigned ModeSelect_b3:3; + unsigned TokenSeed:8; +} TALON_Status_1_General_10ms_t ; +typedef struct _TALON_Status_2_Feedback_20ms_t { + unsigned SensorPositionH:8; + unsigned SensorPositionM:8; + unsigned SensorPositionL:8; + unsigned SensorVelocityH:8; + unsigned SensorVelocityL:8; + unsigned Current_h8:8; + unsigned StckyFault_RevSoftLim:1; + unsigned StckyFault_ForSoftLim:1; + unsigned StckyFault_RevLim:1; + unsigned StckyFault_ForLim:1; + unsigned StckyFault_UnderVoltage:1; + unsigned StckyFault_OverTemp:1; + unsigned Current_l2:2; + unsigned reserved:3; + unsigned Cmd5Allowed:1; + unsigned VelDiv4:1; + unsigned PosDiv8:1; + unsigned ProfileSlotSelect:1; + unsigned BrakeIsEnabled:1; +} TALON_Status_2_Feedback_20ms_t ; +typedef struct _TALON_Status_3_Enc_100ms_t { + unsigned EncPositionH:8; + unsigned EncPositionM:8; + unsigned EncPositionL:8; + unsigned EncVelH:8; + unsigned EncVelL:8; + unsigned EncIndexRiseEventsH:8; + unsigned EncIndexRiseEventsL:8; + unsigned reserved:3; + unsigned VelDiv4:1; + unsigned PosDiv8:1; + unsigned QuadIdxpin:1; + unsigned QuadBpin:1; + unsigned QuadApin:1; +} TALON_Status_3_Enc_100ms_t ; +typedef struct _TALON_Status_4_AinTempVbat_100ms_t { + unsigned AnalogInWithOvH:8; + unsigned AnalogInWithOvM:8; + unsigned AnalogInWithOvL:8; + unsigned AnalogInVelH:8; + unsigned AnalogInVelL:8; + unsigned Temp:8; + unsigned BatteryV:8; + unsigned reserved:6; + unsigned VelDiv4:1; + unsigned PosDiv8:1; +} TALON_Status_4_AinTempVbat_100ms_t ; +typedef struct _TALON_Status_5_Startup_OneShot_t { + unsigned ResetCountH:8; + unsigned ResetCountL:8; + unsigned ResetFlagsH:8; + unsigned ResetFlagsL:8; + unsigned FirmVersH:8; + unsigned FirmVersL:8; +} TALON_Status_5_Startup_OneShot_t ; +typedef struct _TALON_Status_6_Eol_t { + unsigned currentAdcUncal_h2:2; + unsigned reserved1:5; + unsigned SpiCsPin_GadgeteerPin6:1; + unsigned currentAdcUncal_l8:8; + unsigned tempAdcUncal_h2:2; + unsigned reserved2:6; + unsigned tempAdcUncal_l8:8; + unsigned vbatAdcUncal_h2:2; + unsigned reserved3:6; + unsigned vbatAdcUncal_l8:8; + unsigned analogAdcUncal_h2:2; + unsigned reserved4:6; + unsigned analogAdcUncal_l8:8; +} TALON_Status_6_Eol_t ; +typedef struct _TALON_Status_7_Debug_200ms_t { + unsigned TokenizationFails_h8:8; + unsigned TokenizationFails_l8:8; + unsigned LastFailedToken_h8:8; + unsigned LastFailedToken_l8:8; + unsigned TokenizationSucceses_h8:8; + unsigned TokenizationSucceses_l8:8; +} TALON_Status_7_Debug_200ms_t ; +typedef struct _TALON_Status_8_PulseWid_100ms_t { + unsigned PulseWidPositionH:8; + unsigned PulseWidPositionM:8; + unsigned PulseWidPositionL:8; + unsigned reserved:6; + unsigned VelDiv4:1; + unsigned PosDiv8:1; + unsigned PeriodUsM8:8; + unsigned PeriodUsL8:8; + unsigned PulseWidVelH:8; + unsigned PulseWidVelL:8; +} TALON_Status_8_PulseWid_100ms_t ; +typedef struct _TALON_Status_9_MotProfBuffer_100ms_t { + unsigned ActTraj_IsValid:1; //!< '1' if other ActTraj_* signals are valid. '0' if there is no active trajectory pt. + unsigned ActTraj_ProfileSlotSelect:1; + unsigned ActTraj_VelOnly:1; + unsigned ActTraj_IsLast:1; + unsigned OutputType:2; + unsigned HasUnderrun:1; + unsigned IsUnderrun:1; + unsigned NextID:4; + unsigned reserved1:3; + unsigned BufferIsFull:1; + unsigned Count:8; + unsigned ActTraj_VelocityH:8; + unsigned ActTraj_VelocityL:8; + unsigned ActTraj_PositionH:8; + unsigned ActTraj_PositionM:8; + unsigned ActTraj_PositionL:8; +} TALON_Status_9_MotProfBuffer_100ms_t ; +typedef struct _TALON_Param_Request_t { + unsigned ParamEnum:8; +} TALON_Param_Request_t ; +typedef struct _TALON_Param_Response_t { + unsigned ParamEnum:8; + unsigned ParamValueL:8; + unsigned ParamValueML:8; + unsigned ParamValueMH:8; + unsigned ParamValueH:8; +} TALON_Param_Response_t ; + +#endif /* CTRE_FRAMES_H */ diff --git a/hal/lib/Athena/ctre/CanTalonSRX.cpp b/hal/lib/Athena/ctre/CanTalonSRX.cpp index 56e68e1ec1..68d71ff5e4 100644 --- a/hal/lib/Athena/ctre/CanTalonSRX.cpp +++ b/hal/lib/Athena/ctre/CanTalonSRX.cpp @@ -89,10 +89,13 @@ #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) @@ -103,6 +106,7 @@ #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 @@ -117,166 +121,22 @@ 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; -/* encoder/decoders */ -/** control */ -typedef struct _TALON_Control_1_General_10ms_t { - unsigned TokenH:8; - unsigned TokenL:8; - unsigned DemandH:8; - unsigned DemandM:8; - unsigned DemandL:8; - unsigned ProfileSlotSelect:1; - unsigned FeedbackDeviceSelect:4; - unsigned OverrideLimitSwitchEn:3; - unsigned RevFeedbackSensor:1; - unsigned RevMotDuringCloseLoopEn:1; - unsigned OverrideBrakeType:2; - unsigned ModeSelect:4; - unsigned RampThrottle:8; -} TALON_Control_1_General_10ms_t ; -typedef struct _TALON_Control_2_Rates_OneShot_t { - unsigned Status1Ms:8; - unsigned Status2Ms:8; - unsigned Status3Ms:8; - unsigned Status4Ms:8; - unsigned StatusPulWidMs:8; // TALON_Status_8_PulseWid_100ms_t -} TALON_Control_2_Rates_OneShot_t ; -typedef struct _TALON_Control_3_ClearFlags_OneShot_t { - unsigned ZeroFeedbackSensor:1; - unsigned ClearStickyFaults:1; -} TALON_Control_3_ClearFlags_OneShot_t ; -/** status */ -typedef struct _TALON_Status_1_General_10ms_t { - unsigned CloseLoopErrH:8; - unsigned CloseLoopErrM:8; - unsigned CloseLoopErrL:8; - unsigned AppliedThrottle_h3:3; - unsigned Fault_RevSoftLim:1; - unsigned Fault_ForSoftLim:1; - unsigned TokLocked:1; - unsigned LimitSwitchClosedRev:1; - unsigned LimitSwitchClosedFor:1; - unsigned AppliedThrottle_l8:8; - unsigned ModeSelect_h1:1; - unsigned FeedbackDeviceSelect:4; - unsigned LimitSwitchEn:3; - unsigned Fault_HardwareFailure:1; - unsigned Fault_RevLim:1; - unsigned Fault_ForLim:1; - unsigned Fault_UnderVoltage:1; - unsigned Fault_OverTemp:1; - unsigned ModeSelect_b3:3; - unsigned TokenSeed:8; -} TALON_Status_1_General_10ms_t ; -typedef struct _TALON_Status_2_Feedback_20ms_t { - unsigned SensorPositionH:8; - unsigned SensorPositionM:8; - unsigned SensorPositionL:8; - unsigned SensorVelocityH:8; - unsigned SensorVelocityL:8; - unsigned Current_h8:8; - unsigned StckyFault_RevSoftLim:1; - unsigned StckyFault_ForSoftLim:1; - unsigned StckyFault_RevLim:1; - unsigned StckyFault_ForLim:1; - unsigned StckyFault_UnderVoltage:1; - unsigned StckyFault_OverTemp:1; - unsigned Current_l2:2; - unsigned reserved2:4; - unsigned VelDiv4:1; - unsigned PosDiv8:1; - unsigned ProfileSlotSelect:1; - unsigned BrakeIsEnabled:1; -} TALON_Status_2_Feedback_20ms_t ; -typedef struct _TALON_Status_3_Enc_100ms_t { - unsigned EncPositionH:8; - unsigned EncPositionM:8; - unsigned EncPositionL:8; - unsigned EncVelH:8; - unsigned EncVelL:8; - unsigned EncIndexRiseEventsH:8; - unsigned EncIndexRiseEventsL:8; - unsigned reserved:3; - unsigned VelDiv4:1; - unsigned PosDiv8:1; - unsigned QuadIdxpin:1; - unsigned QuadBpin:1; - unsigned QuadApin:1; -} TALON_Status_3_Enc_100ms_t ; -typedef struct _TALON_Status_4_AinTempVbat_100ms_t { - unsigned AnalogInWithOvH:8; - unsigned AnalogInWithOvM:8; - unsigned AnalogInWithOvL:8; - unsigned AnalogInVelH:8; - unsigned AnalogInVelL:8; - unsigned Temp:8; - unsigned BatteryV:8; - unsigned reserved:6; - unsigned VelDiv4:1; - unsigned PosDiv8:1; -} TALON_Status_4_AinTempVbat_100ms_t ; -typedef struct _TALON_Status_5_Startup_OneShot_t { - unsigned ResetCountH:8; - unsigned ResetCountL:8; - unsigned ResetFlagsH:8; - unsigned ResetFlagsL:8; - unsigned FirmVersH:8; - unsigned FirmVersL:8; -} TALON_Status_5_Startup_OneShot_t ; -typedef struct _TALON_Status_6_Eol_t { - unsigned currentAdcUncal_h2:2; - unsigned reserved1:5; - unsigned SpiCsPin_GadgeteerPin6:1; - unsigned currentAdcUncal_l8:8; - unsigned tempAdcUncal_h2:2; - unsigned reserved2:6; - unsigned tempAdcUncal_l8:8; - unsigned vbatAdcUncal_h2:2; - unsigned reserved3:6; - unsigned vbatAdcUncal_l8:8; - unsigned analogAdcUncal_h2:2; - unsigned reserved4:6; - unsigned analogAdcUncal_l8:8; -} TALON_Status_6_Eol_t ; -typedef struct _TALON_Status_7_Debug_200ms_t { - unsigned TokenizationFails_h8:8; - unsigned TokenizationFails_l8:8; - unsigned LastFailedToken_h8:8; - unsigned LastFailedToken_l8:8; - unsigned TokenizationSucceses_h8:8; - unsigned TokenizationSucceses_l8:8; -} TALON_Status_7_Debug_200ms_t ; -typedef struct _TALON_Status_8_PulseWid_100ms_t { - unsigned PulseWidPositionH:8; - unsigned PulseWidPositionM:8; - unsigned PulseWidPositionL:8; - unsigned reserved:6; - unsigned VelDiv4:1; - unsigned PosDiv8:1; - unsigned PeriodUsM8:8; - unsigned PeriodUsL8:8; - unsigned PulseWidVelH:8; - unsigned PulseWidVelL:8; -} TALON_Status_8_PulseWid_100ms_t ; -typedef struct _TALON_Param_Request_t { - unsigned ParamEnum:8; -} TALON_Param_Request_t ; -typedef struct _TALON_Param_Response_t { - unsigned ParamEnum:8; - unsigned ParamValueL:8; - unsigned ParamValueML:8; - unsigned ParamValueMH:8; - unsigned ParamValueH:8; -} TALON_Param_Response_t ; - -CanTalonSRX::CanTalonSRX(int deviceNumber,int controlPeriodMs): CtreCanNode(deviceNumber), _can_h(0), _can_stat(0) +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(_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 ); @@ -284,10 +144,14 @@ CanTalonSRX::CanTalonSRX(int deviceNumber,int controlPeriodMs): CtreCanNode(devi RegisterRx(STATUS_5 | (UINT8)deviceNumber ); RegisterRx(STATUS_6 | (UINT8)deviceNumber ); RegisterRx(STATUS_7 | (UINT8)deviceNumber ); - RegisterTx(CONTROL_1 | (UINT8)deviceNumber, (UINT8)controlPeriodMs); + /* 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 */ @@ -299,6 +163,7 @@ CanTalonSRX::~CanTalonSRX() }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){ @@ -306,6 +171,82 @@ CanTalonSRX::~CanTalonSRX() _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; @@ -385,6 +326,21 @@ CTR_Code CanTalonSRX::SetParamRaw(unsigned paramEnum, int rawBits) 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; @@ -660,6 +616,9 @@ CTR_Code CanTalonSRX::SetStatusFrameRate(unsigned frameEnum, unsigned periodMs) 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; @@ -687,6 +646,286 @@ CTR_Code CanTalonSRX::ClearStickyFaults() 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 eventuall 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 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. + * @param [out] outputEnable zero if motion profile output is disabled, one if enabled, two if executer is in hold state. + * @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) @@ -1005,97 +1244,6 @@ CTR_Code CanTalonSRX::GetFirmVers(int ¶m) param = (int)raw; return rx.err; } -CTR_Code CanTalonSRX::SetDemand(int param) -{ - CtreCanNode::txTask toFill = GetTx(CONTROL_1 | 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(CONTROL_1 | 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(CONTROL_1 | 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(CONTROL_1 | 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(CONTROL_1 | 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(CONTROL_1 | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->ModeSelect = param; - FlushTx(toFill); - return CTR_OKAY; -} -/** - * @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(CONTROL_1 | 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; -} -CTR_Code CanTalonSRX::SetProfileSlotSelect(int param) -{ - CtreCanNode::txTask toFill = GetTx(CONTROL_1 | 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(CONTROL_1 | 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(CONTROL_1 | GetDeviceNumber()); - if (toFill.IsEmpty()) return CTR_UnexpectedArbId; - toFill->RevFeedbackSensor = param ? 1 : 0; - FlushTx(toFill); - return CTR_OKAY; -} CTR_Code CanTalonSRX::GetPulseWidthPosition(int ¶m) { GET_STATUS8(); @@ -1172,6 +1320,97 @@ CTR_Code CanTalonSRX::IsPulseWidthSensorPresent(int ¶m) param = 0; return retval; } +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; +} +/** + * @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; +} +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_Create(int deviceNumber, int controlPeriodMs) @@ -1354,6 +1593,26 @@ 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_GetPulseWidthRiseToFallUs(void *handle, int *param) +{ + return ((CanTalonSRX*)handle)->GetPulseWidthRiseToFallUs(*param); +} +CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param) +{ + return ((CanTalonSRX*)handle)->GetPulseWidthRiseToRiseUs(*param); +} +CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param) +{ + return ((CanTalonSRX*)handle)->IsPulseWidthSensorPresent(*param); +} CTR_Code c_TalonSRX_SetDemand(void *handle, int param) { return ((CanTalonSRX*)handle)->SetDemand(param); @@ -1394,24 +1653,4 @@ CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param) { return ((CanTalonSRX*)handle)->SetRevFeedbackSensor(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_GetPulseWidthRiseToFallUs(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetPulseWidthRiseToFallUs(*param); -} -CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->GetPulseWidthRiseToRiseUs(*param); -} -CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param) -{ - return ((CanTalonSRX*)handle)->IsPulseWidthSensorPresent(*param); -} } diff --git a/hal/lib/Athena/ctre/CtreCanNode.cpp b/hal/lib/Athena/ctre/CtreCanNode.cpp index 18cd24b8a3..0f190a5670 100644 --- a/hal/lib/Athena/ctre/CtreCanNode.cpp +++ b/hal/lib/Athena/ctre/CtreCanNode.cpp @@ -18,20 +18,57 @@ void CtreCanNode::RegisterRx(uint32_t arbId) { /* no need to do anything, we just use new API to poll last received message */ } -void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs) +/** + * Schedule a CAN Frame for periodic transmit. + * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids. + * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit. + * @param dlc Number of bytes to transmit (0 to 8). + * @param initialFrame Ptr to the frame data to schedule for transmitting. Passing null will result + * in defaulting to zero data value. + */ +void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame) { int32_t status = 0; - + if(dlc > 8) + dlc = 8; txJob_t job = {0}; job.arbId = arbId; job.periodMs = periodMs; + job.dlc = dlc; + if(initialFrame){ + /* caller wants to specify original data */ + memcpy(job.toSend, initialFrame, dlc); + } _txJobs[arbId] = job; FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId, job.toSend, - 8, + job.dlc, job.periodMs, &status); } +/** + * Schedule a CAN Frame for periodic transmit. Assume eight byte DLC and zero value for initial transmission. + * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids. + * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit. + */ +void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs) +{ + RegisterTx(arbId,periodMs, 8, 0); +} +/** + * Remove a CAN frame Arbid to stop transmission. + * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids. + */ +void CtreCanNode::UnregisterTx(uint32_t arbId) +{ + /* set period to zero */ + ChangeTxPeriod(arbId, 0); + /* look and remove */ + txJobs_t::iterator iter = _txJobs.find(arbId); + if(iter != _txJobs.end()) { + _txJobs.erase(iter); + } +} timespec diff(const timespec & start, const timespec & end) { timespec temp; @@ -94,8 +131,34 @@ void CtreCanNode::FlushTx(uint32_t arbId) if(iter != _txJobs.end()) FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId, iter->second.toSend, - 8, + iter->second.dlc, iter->second.periodMs, &status); } +/** + * Change the transmit period of an already scheduled CAN frame. + * This keeps the frame payload contents the same without caller having to perform + * a read-modify-write. + * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids. + * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit. + * @return true if scheduled job was found and updated, false if there was no preceding job for the specified arbID. + */ +bool CtreCanNode::ChangeTxPeriod(uint32_t arbId, uint32_t periodMs) +{ + int32_t status = 0; + /* lookup the data bytes and period for this message */ + txJobs_t::iterator iter = _txJobs.find(arbId); + if(iter != _txJobs.end()) { + /* modify th periodMs */ + iter->second.periodMs = periodMs; + /* reinsert into scheduler with the same data bytes, only the period changed. */ + FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId, + iter->second.toSend, + iter->second.dlc, + iter->second.periodMs, + &status); + return true; + } + return false; +} diff --git a/wpilibc/Athena/include/CANSpeedController.h b/wpilibc/Athena/include/CANSpeedController.h index 73ed379417..5ef8cfa250 100644 --- a/wpilibc/Athena/include/CANSpeedController.h +++ b/wpilibc/Athena/include/CANSpeedController.h @@ -20,7 +20,8 @@ class CANSpeedController : public SpeedController { kSpeed = 2, kPosition = 3, kVoltage = 4, - kFollower = 5 // Not supported in Jaguar. + kFollower = 5, // Not supported in Jaguar. + kMotionProfile = 6, // Not supported in Jaguar. }; // Helper function for the ControlMode enum diff --git a/wpilibc/Athena/include/CANTalon.h b/wpilibc/Athena/include/CANTalon.h index 15ebbe258a..19c3f79907 100644 --- a/wpilibc/Athena/include/CANTalon.h +++ b/wpilibc/Athena/include/CANTalon.h @@ -42,9 +42,9 @@ class CANTalon : public MotorSafety, * 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... + 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, @@ -53,6 +53,151 @@ class CANTalon : public MotorSafety, 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. + */ + unsigned 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. + */ + unsigned 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. + */ + unsigned int topBufferRem; + /** + * The number of points in the top trajectory buffer. + */ + unsigned int topBufferCnt; + /** + * The number of points in the low level Talon buffer. + */ + unsigned 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); @@ -167,9 +312,9 @@ class CANTalon : public MotorSafety, /** * 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. + * @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(uint32_t paramEnum, double value); @@ -193,6 +338,69 @@ class CANTalon : public MotorSafety, 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; @@ -216,6 +424,7 @@ class CANTalon : public MotorSafety, kPositionMode = 1, kSpeedMode = 2, kCurrentMode = 3, + kMotionProfileMode = 6, kDisabled = 15 }; @@ -255,10 +464,10 @@ class CANTalon : public MotorSafety, static const unsigned 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. + * 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 */ @@ -271,40 +480,40 @@ class CANTalon : public MotorSafety, */ 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 + * @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. */ int32_t 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 + * @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. */ int32_t 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 + * @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, int32_t 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 + * @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, int32_t nativeVel) const; diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp index fc71e84d65..2b4cd029eb 100644 --- a/wpilibc/Athena/src/CANJaguar.cpp +++ b/wpilibc/Athena/src/CANJaguar.cpp @@ -1010,6 +1010,7 @@ void CANJaguar::SetP(double p) { case kPercentVbus: case kVoltage: case kFollower: + case kMotionProfile: wpi_setWPIErrorWithContext( IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); @@ -1045,6 +1046,7 @@ void CANJaguar::SetI(double i) { case kPercentVbus: case kVoltage: case kFollower: + case kMotionProfile: wpi_setWPIErrorWithContext( IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); @@ -1080,6 +1082,7 @@ void CANJaguar::SetD(double d) { case kPercentVbus: case kVoltage: case kFollower: + case kMotionProfile: wpi_setWPIErrorWithContext( IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); @@ -1516,7 +1519,7 @@ void CANJaguar::SetControlMode(ControlMode controlMode) { // Disable the previous mode DisableControl(); - if (controlMode == kFollower) + if ((controlMode == kFollower) || (controlMode == kMotionProfile)) wpi_setWPIErrorWithContext(IncompatibleMode, "The Jaguar only supports Current, Voltage, " "Position, Speed, and Percent (Throttle) " diff --git a/wpilibc/Athena/src/CANTalon.cpp b/wpilibc/Athena/src/CANTalon.cpp index f9372129d5..765c7c4289 100644 --- a/wpilibc/Athena/src/CANTalon.cpp +++ b/wpilibc/Athena/src/CANTalon.cpp @@ -156,6 +156,9 @@ void CANTalon::Set(float value, uint8_t syncGroup) { double milliamperes = (m_isInverted ? -value : value) * 1000.0; /* mA*/ status = m_impl->SetDemand(milliamperes); } break; + case CANSpeedController::kMotionProfile: { + status = m_impl->SetDemand((int)value); + } break; default: wpi_setWPIErrorWithContext( IncompatibleMode, @@ -1430,6 +1433,9 @@ void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) { 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((int)kDisabled); @@ -1668,6 +1674,136 @@ void CANTalon::EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge) 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 */ + int32_t targPos = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position); + int32_t targVel = ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity); + /* bounds check signals that require it */ + uint32_t profileSlotSelect = (trajPt.profileSlotSelect) ? 1 : 0; + uint8_t 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; + int32_t 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, getHALErrorMessage(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. diff --git a/wpilibj/src/athena/cpp/lib/CanTalonSRXJNI.cpp b/wpilibj/src/athena/cpp/lib/CanTalonSRXJNI.cpp index 0acf12db95..c2ddb1373d 100644 --- a/wpilibj/src/athena/cpp/lib/CanTalonSRXJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/CanTalonSRXJNI.cpp @@ -904,7 +904,49 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 } -SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_10(JNIEnv *jenv, jclass jcls, jint jarg1, jint jarg2) { +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kDefaultEnablePeriodMs_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kDefaultEnablePeriodMs; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kDefaultControl6PeriodMs_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kDefaultControl6PeriodMs; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_10(JNIEnv *jenv, jclass jcls, jint jarg1, jint jarg2, jint jarg3) { + jlong jresult = 0 ; + int arg1 ; + int arg2 ; + int arg3 ; + CanTalonSRX *result = 0 ; + + (void)jenv; + (void)jcls; + arg1 = (int)jarg1; + arg2 = (int)jarg2; + arg3 = (int)jarg3; + result = (CanTalonSRX *)new CanTalonSRX(arg1,arg2,arg3); + *(CanTalonSRX **)&jresult = result; + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_11(JNIEnv *jenv, jclass jcls, jint jarg1, jint jarg2) { jlong jresult = 0 ; int arg1 ; int arg2 ; @@ -920,7 +962,7 @@ SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalo } -SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_11(JNIEnv *jenv, jclass jcls, jint jarg1) { +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_12(JNIEnv *jenv, jclass jcls, jint jarg1) { jlong jresult = 0 ; int arg1 ; CanTalonSRX *result = 0 ; @@ -934,7 +976,7 @@ SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalo } -SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_12(JNIEnv *jenv, jclass jcls) { +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_new_1CanTalonSRX_1_1SWIG_13(JNIEnv *jenv, jclass jcls) { jlong jresult = 0 ; CanTalonSRX *result = 0 ; @@ -1041,6 +1083,18 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 } +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMode_1MotionProfile_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMode_MotionProfile; + jresult = (jint)result; + return jresult; +} + + SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMode_1NoDrive_1get(JNIEnv *jenv, jclass jcls) { jint jresult = 0 ; int result; @@ -1305,6 +1359,114 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 } +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kStatusFrame_1MotionProfile_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kStatusFrame_MotionProfile; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProfileFlag_1ActTraj_1IsValid_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProfileFlag_ActTraj_IsValid; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProfileFlag_1HasUnderrun_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProfileFlag_HasUnderrun; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProfileFlag_1IsUnderrun_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProfileFlag_IsUnderrun; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProfileFlag_1ActTraj_1IsLast_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProfileFlag_ActTraj_IsLast; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProfileFlag_1ActTraj_1VelOnly_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProfileFlag_ActTraj_VelOnly; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProf_1Disabled_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProf_Disabled; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProf_1Enable_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProf_Enable; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1kMotionProf_1Hold_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + int result; + + (void)jenv; + (void)jcls; + result = (int)CanTalonSRX::kMotionProf_Hold; + jresult = (jint)result; + return jresult; +} + + SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eProfileParamSlot0_1P_1get(JNIEnv *jenv, jclass jcls) { jint jresult = 0 ; CanTalonSRX::_param_t result; @@ -2107,6 +2269,7 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 return jresult; } + SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eStatus3FrameRate_1get(JNIEnv *jenv, jclass jcls) { jint jresult = 0 ; @@ -2119,6 +2282,7 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 return jresult; } + SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eStatus4FrameRate_1get(JNIEnv *jenv, jclass jcls) { jint jresult = 0 ; @@ -2319,6 +2483,54 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 } +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eStatus9FrameRate_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + CanTalonSRX::_param_t result; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX::_param_t)CanTalonSRX::eStatus9FrameRate; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eMotionProfileHasUnderrunErr_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + CanTalonSRX::_param_t result; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX::_param_t)CanTalonSRX::eMotionProfileHasUnderrunErr; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eReserved120_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + CanTalonSRX::_param_t result; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX::_param_t)CanTalonSRX::eReserved120; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eLegacyControlMode_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + CanTalonSRX::_param_t result; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX::_param_t)CanTalonSRX::eLegacyControlMode; + jresult = (jint)result; + return jresult; +} + + SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetParam(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2, jdouble jarg3) { jlong jresult = 0 ; CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; @@ -2515,6 +2727,23 @@ SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_ } +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetVoltageCompensationRate(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jdouble jarg2) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + double arg2 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (double)jarg2; + result = (arg1)->SetVoltageCompensationRate(arg2); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetSensorPosition(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { jlong jresult = 0 ; CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; @@ -2875,6 +3104,165 @@ SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_ } +SWIGEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1ChangeMotionControlFramePeriod(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + uint32_t arg2 ; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = jarg2; + (arg1)->ChangeMotionControlFramePeriod(arg2); +} + + +SWIGEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1ClearMotionProfileTrajectories(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_) { + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + (arg1)->ClearMotionProfileTrajectories(); +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetMotionProfileTopLevelBufferCount(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_) { + jint jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + uint32_t result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + result = (arg1)->GetMotionProfileTopLevelBufferCount(); + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1IsMotionProfileTopLevelBufferFull(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_) { + jboolean jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + bool result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + result = (bool)(arg1)->IsMotionProfileTopLevelBufferFull(); + jresult = (jboolean)result; + return jresult; +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1PushMotionProfileTrajectory(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2, jint jarg3, jint jarg4, jint jarg5, jint jarg6, jint jarg7, jint jarg8) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + int arg2 ; + int arg3 ; + int arg4 ; + int arg5 ; + int arg6 ; + int arg7 ; + int arg8 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = (int)jarg2; + arg3 = (int)jarg3; + arg4 = (int)jarg4; + arg5 = (int)jarg5; + arg6 = (int)jarg6; + arg7 = (int)jarg7; + arg8 = (int)jarg8; + + + result = (arg1)->PushMotionProfileTrajectory(arg2,arg3,arg4,arg5,arg6,arg7,arg8); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + +SWIGEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1ProcessMotionProfileBuffer(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_) { + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + (arg1)->ProcessMotionProfileBuffer(); +} + + +SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetMotionProfileStatus(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2, jlong jarg3, jlong jarg4, jlong jarg5, jlong jarg6, jlong jarg7, jlong jarg8, jlong jarg9) { + jlong jresult = 0 ; + CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; + uint32_t *arg2 = 0 ; + uint32_t *arg3 = 0 ; + int32_t *arg4 = 0 ; + int32_t *arg5 = 0 ; + uint32_t *arg6 = 0 ; + uint32_t *arg7 = 0 ; + uint32_t *arg8 = 0 ; + uint32_t *arg9 = 0 ; + CTR_Code result; + + (void)jenv; + (void)jcls; + (void)jarg1_; + arg1 = *(CanTalonSRX **)&jarg1; + arg2 = *(uint32_t **)&jarg2; + if (!arg2) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "uint32_t & reference is null"); + return 0; + } + arg3 = *(uint32_t **)&jarg3; + if (!arg3) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "uint32_t & reference is null"); + return 0; + } + arg4 = *(int32_t **)&jarg4; + if (!arg4) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int32_t & reference is null"); + return 0; + } + arg5 = *(int32_t **)&jarg5; + if (!arg5) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "int32_t & reference is null"); + return 0; + } + arg6 = *(uint32_t **)&jarg6; + if (!arg6) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "uint32_t & reference is null"); + return 0; + } + arg7 = *(uint32_t **)&jarg7; + if (!arg7) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "uint32_t & reference is null"); + return 0; + } + arg8 = *(uint32_t **)&jarg8; + if (!arg8) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "uint32_t & reference is null"); + return 0; + } + arg9 = *(uint32_t **)&jarg9; + if (!arg9) { + SWIG_JavaThrowException(jenv, SWIG_JavaNullPointerException, "uint32_t & reference is null"); + return 0; + } + result = (arg1)->GetMotionProfileStatus(*arg2,*arg3,*arg4,*arg5,*arg6,*arg7,*arg8,*arg9); + *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); + return jresult; +} + + SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1GetFault_1OverTemp(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jlong jarg2) { jlong jresult = 0 ; CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; @@ -3788,23 +4176,6 @@ SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_ } -SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetVoltageCompensationRate(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jdouble jarg2) { - jlong jresult = 0 ; - CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; - double arg2 ; - CTR_Code result; - - (void)jenv; - (void)jcls; - (void)jarg1_; - arg1 = *(CanTalonSRX **)&jarg1; - arg2 = (double)jarg2; - result = (arg1)->SetVoltageCompensationRate(arg2); - *(CTR_Code **)&jresult = new CTR_Code((const CTR_Code &)result); - return jresult; -} - - SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetRevFeedbackSensor(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2) { jlong jresult = 0 ; CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java index 2a5b230e86..38910f5394 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.communication.UsageReporting; import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_double; import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_int; +import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_uint32_t; +import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_int32_t; import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_CTR_Code; import edu.wpi.first.wpilibj.tables.ITable; import edu.wpi.first.wpilibj.tables.ITableListener; @@ -31,7 +33,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont private 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), Disabled(15); + PercentVbus(0), Position(1), Speed(2), Current(3), Voltage(4), Follower(5), MotionProfile(6), Disabled(15); public final int value; @@ -114,7 +116,173 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont 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); + public int value; + public static SetValueMotionProfile valueOf(int value) { + for (SetValueMotionProfile mode : values()) { + if (mode.value == value) { + return mode; + } + } + return null; + } + + private SetValueMotionProfile(int value) { + this.value = value; + } + } + /** + * Motion Profile Trajectory Point + * This is simply a data transer object. + */ + 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. + */ + 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; + }; + /** preallocated for Motion Profile Status fetching. This is more efficient + * then creating them on every call. */ + private long _flagsPtr = CanTalonJNI.new_intp(); + private long _profileSlotSelectPtr = CanTalonJNI.new_intp(); + private long _targPosPtr = CanTalonJNI.new_intp(); + private long _targVelPtr = CanTalonJNI.new_intp(); + private long _topBufferRemPtr = CanTalonJNI.new_intp(); + private long _topBufferCntPtr = CanTalonJNI.new_intp(); + private long _btmBufferCntPtr = CanTalonJNI.new_intp(); + private long _outputEnablePtr = CanTalonJNI.new_intp(); private CanTalonSRX m_impl; private TalonControlMode m_controlMode; @@ -148,7 +316,10 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont * 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_impl = new CanTalonSRX(deviceNumber); @@ -162,7 +333,12 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont setProfile(m_profile); applyControlMode(TalonControlMode.PercentVbus); } - + /** + * 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; m_impl = new CanTalonSRX(deviceNumber, controlPeriodMs); /* @@ -180,6 +356,29 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont setProfile(m_profile); applyControlMode(TalonControlMode.PercentVbus); } + /** + * 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_impl = 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); + } @Override public void pidWrite(double output) { @@ -253,6 +452,9 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont double milliamperes = (isInverted ? -outputValue : outputValue) * 1000.0; /* mA*/ m_impl.SetDemand((int)milliamperes); break; + case MotionProfile: + m_impl.SetDemand((int) outputValue); + break; default: break; } @@ -582,15 +784,15 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont /** * 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. + * mA for Curent closed loop. + * Talon Native Units for position and velocity. */ public void setAllowableClosedLoopErr(int allowableCloseLoopError) { if(m_profile == 0){ - setParameter(CanTalonSRX.param_t.eProfileParamSlot0_AllowableClosedLoopErr, (double)allowableCloseLoopError); + setParameter(CanTalonSRX.param_t.eProfileParamSlot0_AllowableClosedLoopErr, (double)allowableCloseLoopError); }else{ - setParameter(CanTalonSRX.param_t.eProfileParamSlot1_AllowableClosedLoopErr, (double)allowableCloseLoopError); + setParameter(CanTalonSRX.param_t.eProfileParamSlot1_AllowableClosedLoopErr, (double)allowableCloseLoopError); } } @@ -1185,17 +1387,17 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont */ public void configMaxOutputVoltage(double voltage) { /* config peak throttle when in closed-loop mode in the fwd and rev direction. */ - configPeakOutputVoltage(voltage, -voltage); + configPeakOutputVoltage(voltage, -voltage); } public void configPeakOutputVoltage(double forwardVoltage,double reverseVoltage) { /* bounds checking */ if(forwardVoltage > 12) - forwardVoltage = 12; + forwardVoltage = 12; else if(forwardVoltage < 0) forwardVoltage = 0; if(reverseVoltage > 0) - reverseVoltage = 0; + reverseVoltage = 0; else if(reverseVoltage < -12) reverseVoltage = -12; /* config calls */ @@ -1205,11 +1407,11 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont public void configNominalOutputVoltage(double forwardVoltage,double reverseVoltage) { /* bounds checking */ if(forwardVoltage > 12) - forwardVoltage = 12; + forwardVoltage = 12; else if(forwardVoltage < 0) forwardVoltage = 0; if(reverseVoltage > 0) - reverseVoltage = 0; + reverseVoltage = 0; else if(reverseVoltage < -12) reverseVoltage = -12; /* config calls */ @@ -1229,14 +1431,14 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont * be used for testing future features. */ public double getParameter(CanTalonSRX.param_t paramEnum) { - /* transmit a request for this param */ + /* transmit a request for this param */ m_impl.RequestParam(paramEnum); /* Briefly wait for new values from the Talon. */ Timer.delay(kDelayForSolicitedSignals); - /* poll out latest response value */ + /* poll out latest response value */ long pp = CanTalonJNI.new_doublep(); SWIGTYPE_p_CTR_Code status = m_impl.GetParamResponse(paramEnum, new SWIGTYPE_p_double(pp, true)); - /* pass latest value back to caller */ + /* pass latest value back to caller */ return CanTalonJNI.doublep_value(pp); } public void clearStickyFaults() { @@ -1363,183 +1565,312 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont } /** * @return Number of native units per rotation if scaling info is available. - * Zero if scaling information is not available. + * Zero if scaling information is not available. */ 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; - } - /* if scaling info is not available give caller zero */ - if(false == scalingAvail) - return 0; - return retval; + 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; + } + /* 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. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev + * @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) { - /* 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; + /* 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. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev + * @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) { - /* 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; + /* 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. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev + * @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) { - /* 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; + /* 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. - * @see configPotentiometerTurns - * @see configEncoderCodesPerRev + * @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, 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; + 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. + * 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(CanTalonSRX.param_t.eQuadIdxPolarity,risingEdge ? 1 : 0); - setParameter(CanTalonSRX.param_t.eClearPositionOnIdx,1); - }else{ - /* disable the feature first, then update the edge polarity. */ - setParameter(CanTalonSRX.param_t.eClearPositionOnIdx,0); - setParameter(CanTalonSRX.param_t.eQuadIdxPolarity,risingEdge ? 1 : 0); - } + /* enable the feature, update the edge polarity first to ensure + it is correct before the feature is enabled. */ + setParameter(CanTalonSRX.param_t.eQuadIdxPolarity,risingEdge ? 1 : 0); + setParameter(CanTalonSRX.param_t.eClearPositionOnIdx,1); + }else{ + /* disable the feature first, then update the edge polarity. */ + setParameter(CanTalonSRX.param_t.eClearPositionOnIdx,0); + setParameter(CanTalonSRX.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) { + 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. + */ + public void 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. + */ + public int 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 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 eventuall 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. + */ + 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 */ + m_impl.PushMotionProfileTrajectory(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 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. + */ + public void processMotionProfileBuffer() { + m_impl.ProcessMotionProfileBuffer(); + } + /** + * 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 [out] motionProfileStatus 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) { + /* retrieve all motion profile signals from status frame */ + SWIGTYPE_p_uint32_t _flagsSwig = new SWIGTYPE_p_uint32_t(_flagsPtr, true); + SWIGTYPE_p_uint32_t _profileSlotSelectSwig = new SWIGTYPE_p_uint32_t(_profileSlotSelectPtr, true); + SWIGTYPE_p_int32_t _targPosSwig = new SWIGTYPE_p_int32_t(_targPosPtr, true); + SWIGTYPE_p_int32_t _targVelSwig = new SWIGTYPE_p_int32_t(_targVelPtr, true); + SWIGTYPE_p_uint32_t _topBufferRemSwig = new SWIGTYPE_p_uint32_t(_topBufferRemPtr, true); + SWIGTYPE_p_uint32_t _topBufferCntSwig = new SWIGTYPE_p_uint32_t(_topBufferCntPtr, true); + SWIGTYPE_p_uint32_t _btmBufferCntSwig = new SWIGTYPE_p_uint32_t(_btmBufferCntPtr, true); + SWIGTYPE_p_uint32_t _outputEnableSwig = new SWIGTYPE_p_uint32_t(_outputEnablePtr, true); + /* call native routine then empty swig values into caller's motionProfileStatus */ + m_impl.GetMotionProfileStatus(_flagsSwig, _profileSlotSelectSwig, _targPosSwig, _targVelSwig, _topBufferRemSwig, _topBufferCntSwig, _btmBufferCntSwig, _outputEnableSwig); + /* cache the params that needs processing */ + int flags = CanTalonJNI.intp_value(_flagsPtr); + int targPos = CanTalonJNI.intp_value(_targPosPtr); + int targVel = CanTalonJNI.intp_value(_targVelPtr); + /* completely update the caller's structure */ + motionProfileStatus.topBufferRem = CanTalonJNI.intp_value(_topBufferRemPtr); + motionProfileStatus.topBufferCnt = CanTalonJNI.intp_value(_topBufferCntPtr); + motionProfileStatus.btmBufferCnt = CanTalonJNI.intp_value(_btmBufferCntPtr); + motionProfileStatus.hasUnderrun = ((flags & CanTalonSRX.kMotionProfileFlag_HasUnderrun)>0) ? true :false; + motionProfileStatus.isUnderrun = ((flags & CanTalonSRX.kMotionProfileFlag_IsUnderrun)>0) ? true :false; + motionProfileStatus.activePointValid = ((flags & CanTalonSRX.kMotionProfileFlag_ActTraj_IsValid)>0) ? true :false; + motionProfileStatus.activePoint.isLastPoint = ((flags & CanTalonSRX.kMotionProfileFlag_ActTraj_IsLast)>0) ? true :false; + motionProfileStatus.activePoint.velocityOnly = ((flags & CanTalonSRX.kMotionProfileFlag_ActTraj_VelOnly)>0) ? true :false; + motionProfileStatus.activePoint.position = ScaleNativeUnitsToRotations(m_feedbackDevice, targPos); + motionProfileStatus.activePoint.velocity = ScaleNativeUnitsToRpm(m_feedbackDevice, targVel); + motionProfileStatus.activePoint.profileSlotSelect = CanTalonJNI.intp_value(_profileSlotSelectPtr); + motionProfileStatus.outputEnable = SetValueMotionProfile.valueOf(CanTalonJNI.intp_value(_outputEnablePtr)); + 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 */ + } + /** + * 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(CanTalonSRX.param_t.eMotionProfileHasUnderrunErr, 0); } @Override public void setExpiration(double timeout) { diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CanTalonSRX.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CanTalonSRX.java index 078d0d17f9..97afb61a79 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CanTalonSRX.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CanTalonSRX.java @@ -37,16 +37,20 @@ public class CanTalonSRX extends CtreCanNode { super.delete(); } + public CanTalonSRX(int deviceNumber, int controlPeriodMs, int enablePeriodMs) { + this(CanTalonJNI.new_CanTalonSRX__SWIG_0(deviceNumber, controlPeriodMs, enablePeriodMs), true); + } + public CanTalonSRX(int deviceNumber, int controlPeriodMs) { - this(CanTalonJNI.new_CanTalonSRX__SWIG_0(deviceNumber, controlPeriodMs), true); + this(CanTalonJNI.new_CanTalonSRX__SWIG_1(deviceNumber, controlPeriodMs), true); } public CanTalonSRX(int deviceNumber) { - this(CanTalonJNI.new_CanTalonSRX__SWIG_1(deviceNumber), true); + this(CanTalonJNI.new_CanTalonSRX__SWIG_2(deviceNumber), true); } public CanTalonSRX() { - this(CanTalonJNI.new_CanTalonSRX__SWIG_2(), true); + this(CanTalonJNI.new_CanTalonSRX__SWIG_3(), true); } public void Set(double value) { @@ -104,8 +108,8 @@ public class CanTalonSRX extends CtreCanNode { slotIdx, closeLoopRampRate), true); } - public SWIGTYPE_p_CTR_Code SetVoltageCompensationRate(double vpers) { - return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetVoltageCompensationRate(swigCPtr, this,vpers), true); + public SWIGTYPE_p_CTR_Code SetVoltageCompensationRate(double voltagePerMs) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_SetVoltageCompensationRate(swigCPtr, this, voltagePerMs), true); } public SWIGTYPE_p_CTR_Code SetSensorPosition(int pos) { @@ -162,9 +166,9 @@ public class CanTalonSRX extends CtreCanNode { slotIdx, SWIGTYPE_p_int.getCPtr(closeLoopRampRate)), true); } - public SWIGTYPE_p_CTR_Code GetVoltageCompensationRate(SWIGTYPE_p_double vpers) { + public SWIGTYPE_p_CTR_Code GetVoltageCompensationRate(SWIGTYPE_p_double voltagePerMs) { return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetVoltageCompensationRate(swigCPtr, this, - SWIGTYPE_p_double.getCPtr(vpers)), true); + SWIGTYPE_p_double.getCPtr(voltagePerMs)), true); } public SWIGTYPE_p_CTR_Code GetForwardSoftLimit(SWIGTYPE_p_int forwardLimit) { @@ -196,6 +200,34 @@ public class CanTalonSRX extends CtreCanNode { return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_ClearStickyFaults(swigCPtr, this), true); } + + public void ChangeMotionControlFramePeriod(int periodMs) { + CanTalonJNI.CanTalonSRX_ChangeMotionControlFramePeriod(swigCPtr, this, periodMs); + } + + public void ClearMotionProfileTrajectories() { + CanTalonJNI.CanTalonSRX_ClearMotionProfileTrajectories(swigCPtr, this); + } + + public int GetMotionProfileTopLevelBufferCount() { + return CanTalonJNI.CanTalonSRX_GetMotionProfileTopLevelBufferCount(swigCPtr, this); + } + + public boolean IsMotionProfileTopLevelBufferFull() { + return CanTalonJNI.CanTalonSRX_IsMotionProfileTopLevelBufferFull(swigCPtr, this); + } + + public SWIGTYPE_p_CTR_Code PushMotionProfileTrajectory(int targPos, int targVel, int profileSlotSelect, int timeDurMs, int velOnly, int isLastPoint, int zeroPos) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_PushMotionProfileTrajectory(swigCPtr, this, targPos, targVel, profileSlotSelect, timeDurMs, velOnly, isLastPoint, zeroPos), true); + } + + public void ProcessMotionProfileBuffer() { + CanTalonJNI.CanTalonSRX_ProcessMotionProfileBuffer(swigCPtr, this); + } + + public SWIGTYPE_p_CTR_Code GetMotionProfileStatus(SWIGTYPE_p_uint32_t flags, SWIGTYPE_p_uint32_t profileSlotSelect, SWIGTYPE_p_int32_t targPos, SWIGTYPE_p_int32_t targVel, SWIGTYPE_p_uint32_t topBufferRemaining, SWIGTYPE_p_uint32_t topBufferCnt, SWIGTYPE_p_uint32_t btmBufferCnt, SWIGTYPE_p_uint32_t outputEnable) { + return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetMotionProfileStatus(swigCPtr, this, SWIGTYPE_p_uint32_t.getCPtr(flags), SWIGTYPE_p_uint32_t.getCPtr(profileSlotSelect), SWIGTYPE_p_int32_t.getCPtr(targPos), SWIGTYPE_p_int32_t.getCPtr(targVel), SWIGTYPE_p_uint32_t.getCPtr(topBufferRemaining), SWIGTYPE_p_uint32_t.getCPtr(topBufferCnt), SWIGTYPE_p_uint32_t.getCPtr(btmBufferCnt), SWIGTYPE_p_uint32_t.getCPtr(outputEnable)), true); + } public SWIGTYPE_p_CTR_Code GetFault_OverTemp(SWIGTYPE_p_int param) { return new SWIGTYPE_p_CTR_Code(CanTalonJNI.CanTalonSRX_GetFault_OverTemp(swigCPtr, this, SWIGTYPE_p_int.getCPtr(param)), true); @@ -448,6 +480,8 @@ public class CanTalonSRX extends CtreCanNode { public final static int kDefaultControlPeriodMs = CanTalonJNI .CanTalonSRX_kDefaultControlPeriodMs_get(); + public final static int kDefaultEnablePeriodMs = CanTalonJNI.CanTalonSRX_kDefaultEnablePeriodMs_get(); + public final static int kDefaultControl6PeriodMs = CanTalonJNI.CanTalonSRX_kDefaultControl6PeriodMs_get(); public final static int kMode_DutyCycle = CanTalonJNI.CanTalonSRX_kMode_DutyCycle_get(); public final static int kMode_PositionCloseLoop = CanTalonJNI .CanTalonSRX_kMode_PositionCloseLoop_get(); @@ -457,6 +491,7 @@ public class CanTalonSRX extends CtreCanNode { .CanTalonSRX_kMode_CurrentCloseLoop_get(); public final static int kMode_VoltCompen = CanTalonJNI.CanTalonSRX_kMode_VoltCompen_get(); public final static int kMode_SlaveFollower = CanTalonJNI.CanTalonSRX_kMode_SlaveFollower_get(); + public final static int kMode_MotionProfile = CanTalonJNI.CanTalonSRX_kMode_MotionProfile_get(); public final static int kMode_NoDrive = CanTalonJNI.CanTalonSRX_kMode_NoDrive_get(); public final static int kLimitSwitchOverride_UseDefaultsFromFlash = CanTalonJNI .CanTalonSRX_kLimitSwitchOverride_UseDefaultsFromFlash_get(); @@ -499,6 +534,15 @@ public class CanTalonSRX extends CtreCanNode { public final static int kStatusFrame_PulseWidthMeas = CanTalonJNI .CanTalonSRX_kStatusFrame_PulseWidthMeas_get(); + public final static int kStatusFrame_MotionProfile = CanTalonJNI.CanTalonSRX_kStatusFrame_MotionProfile_get(); + public final static int kMotionProfileFlag_ActTraj_IsValid = CanTalonJNI.CanTalonSRX_kMotionProfileFlag_ActTraj_IsValid_get(); + public final static int kMotionProfileFlag_HasUnderrun = CanTalonJNI.CanTalonSRX_kMotionProfileFlag_HasUnderrun_get(); + public final static int kMotionProfileFlag_IsUnderrun = CanTalonJNI.CanTalonSRX_kMotionProfileFlag_IsUnderrun_get(); + public final static int kMotionProfileFlag_ActTraj_IsLast = CanTalonJNI.CanTalonSRX_kMotionProfileFlag_ActTraj_IsLast_get(); + public final static int kMotionProfileFlag_ActTraj_VelOnly = CanTalonJNI.CanTalonSRX_kMotionProfileFlag_ActTraj_VelOnly_get(); + public final static int kMotionProf_Disabled = CanTalonJNI.CanTalonSRX_kMotionProf_Disabled_get(); + public final static int kMotionProf_Enable = CanTalonJNI.CanTalonSRX_kMotionProf_Enable_get(); + public final static int kMotionProf_Hold = CanTalonJNI.CanTalonSRX_kMotionProf_Hold_get(); public final static class param_t { public final static CanTalonSRX.param_t eProfileParamSlot0_P = new CanTalonSRX.param_t( "eProfileParamSlot0_P", CanTalonJNI.CanTalonSRX_eProfileParamSlot0_P_get()); @@ -683,6 +727,11 @@ public class CanTalonSRX extends CtreCanNode { CanTalonJNI.CanTalonSRX_eProfileParamVcompRate_get()); public final static CanTalonSRX.param_t eProfileParamSlot1_AllowableClosedLoopErr = new CanTalonSRX.param_t("eProfileParamSlot1_AllowableClosedLoopErr", CanTalonJNI.CanTalonSRX_eProfileParamSlot1_AllowableClosedLoopErr_get()); + public final static CanTalonSRX.param_t eStatus9FrameRate = new CanTalonSRX.param_t("eStatus9FrameRate", +CanTalonJNI.CanTalonSRX_eStatus9FrameRate_get()); + public final static CanTalonSRX.param_t eMotionProfileHasUnderrunErr = new CanTalonSRX.param_t("eMotionProfileHasUnderrunErr", CanTalonJNI.CanTalonSRX_eMotionProfileHasUnderrunErr_get()); + public final static CanTalonSRX.param_t eReserved120 = new CanTalonSRX.param_t("eReserved120", CanTalonJNI.CanTalonSRX_eReserved120_get()); + public final static CanTalonSRX.param_t eLegacyControlMode = new CanTalonSRX.param_t("eLegacyControlMode", CanTalonJNI.CanTalonSRX_eLegacyControlMode_get()); public final int swigValue() { return swigValue; @@ -736,7 +785,15 @@ public class CanTalonSRX extends CtreCanNode { eLimitSwitchClosedFor, eLimitSwitchClosedRev, eSensorPosition, eSensorVelocity, eCurrent, eBrakeIsEnabled, eEncPosition, eEncVel, eEncIndexRiseEvents, eQuadApin, eQuadBpin, eQuadIdxpin, eAnalogInWithOv, eAnalogInVel, eTemp, eBatteryV, eResetCount, eResetFlags, - eFirmVers, eSettingsChanged, eQuadFilterEn, ePidIaccum}; + eFirmVers, eSettingsChanged, eQuadFilterEn, ePidIaccum, + eStatus1FrameRate, eStatus2FrameRate, eStatus3FrameRate, + eStatus4FrameRate, eStatus6FrameRate, eStatus7FrameRate, + eClearPositionOnIdx, ePeakPosOutput, eNominalPosOutput, + ePeakNegOutput, eNominalNegOutput, eQuadIdxPolarity, + eStatus8FrameRate, eAllowPosOverflow, eProfileParamSlot0_AllowableClosedLoopErr, + eNumberPotTurns, eNumberEncoderCPR, ePwdPosition, eAinPosition, eProfileParamVcompRate, + eProfileParamSlot1_AllowableClosedLoopErr, eStatus9FrameRate, eMotionProfileHasUnderrunErr, + eReserved120, eLegacyControlMode }; private static int swigNext = 0; private final int swigValue; private final String swigName; 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 index b194cf6579..6cd8cd5965 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java @@ -89,11 +89,17 @@ public class CanTalonJNI { public final static native int CanTalonSRX_kDefaultControlPeriodMs_get(); - public final static native long new_CanTalonSRX__SWIG_0(int jarg1, int jarg2); + public final static native int CanTalonSRX_kDefaultEnablePeriodMs_get(); - public final static native long new_CanTalonSRX__SWIG_1(int jarg1); + public final static native int CanTalonSRX_kDefaultControl6PeriodMs_get(); - public final static native long new_CanTalonSRX__SWIG_2(); + public final static native long new_CanTalonSRX__SWIG_0(int jarg1, int jarg2, int jarg3); + + public final static native long new_CanTalonSRX__SWIG_1(int jarg1, int jarg2); + + public final static native long new_CanTalonSRX__SWIG_2(int jarg1); + + public final static native long new_CanTalonSRX__SWIG_3(); public final static native void delete_CanTalonSRX(long jarg1); @@ -111,6 +117,8 @@ public class CanTalonJNI { public final static native int CanTalonSRX_kMode_SlaveFollower_get(); + public final static native int CanTalonSRX_kMode_MotionProfile_get(); + public final static native int CanTalonSRX_kMode_NoDrive_get(); public final static native int CanTalonSRX_kLimitSwitchOverride_UseDefaultsFromFlash_get(); @@ -155,6 +163,24 @@ public class CanTalonJNI { public final static native int CanTalonSRX_kStatusFrame_PulseWidthMeas_get(); + public final static native int CanTalonSRX_kStatusFrame_MotionProfile_get(); + + public final static native int CanTalonSRX_kMotionProfileFlag_ActTraj_IsValid_get(); + + public final static native int CanTalonSRX_kMotionProfileFlag_HasUnderrun_get(); + + public final static native int CanTalonSRX_kMotionProfileFlag_IsUnderrun_get(); + + public final static native int CanTalonSRX_kMotionProfileFlag_ActTraj_IsLast_get(); + + public final static native int CanTalonSRX_kMotionProfileFlag_ActTraj_VelOnly_get(); + + public final static native int CanTalonSRX_kMotionProf_Disabled_get(); + + public final static native int CanTalonSRX_kMotionProf_Enable_get(); + + public final static native int CanTalonSRX_kMotionProf_Hold_get(); + public final static native int CanTalonSRX_eProfileParamSlot0_P_get(); public final static native int CanTalonSRX_eProfileParamSlot0_I_get(); @@ -327,6 +353,14 @@ public class CanTalonJNI { public final static native int CanTalonSRX_eProfileParamSlot1_AllowableClosedLoopErr_get(); + public final static native int CanTalonSRX_eStatus9FrameRate_get(); + + public final static native int CanTalonSRX_eMotionProfileHasUnderrunErr_get(); + + public final static native int CanTalonSRX_eReserved120_get(); + + public final static native int CanTalonSRX_eLegacyControlMode_get(); + public final static native long CanTalonSRX_SetParam(long jarg1, CanTalonSRX jarg1_, int jarg2, double jarg3); @@ -409,6 +443,20 @@ public class CanTalonJNI { public final static native long CanTalonSRX_ClearStickyFaults(long jarg1, CanTalonSRX jarg1_); + public final static native void CanTalonSRX_ChangeMotionControlFramePeriod(long jarg1, CanTalonSRX jarg1_, int jarg2); + + public final static native void CanTalonSRX_ClearMotionProfileTrajectories(long jarg1, CanTalonSRX jarg1_); + + public final static native int CanTalonSRX_GetMotionProfileTopLevelBufferCount(long jarg1, CanTalonSRX jarg1_); + + public final static native boolean CanTalonSRX_IsMotionProfileTopLevelBufferFull(long jarg1, CanTalonSRX jarg1_); + + public final static native long CanTalonSRX_PushMotionProfileTrajectory(long jarg1, CanTalonSRX jarg1_, int jarg2, int jarg3, int jarg4, int jarg5, int jarg6, int jarg7, int jarg8); + + public final static native void CanTalonSRX_ProcessMotionProfileBuffer(long jarg1, CanTalonSRX jarg1_); + + public final static native long CanTalonSRX_GetMotionProfileStatus(long jarg1, CanTalonSRX jarg1_, long jarg2, long jarg3, long jarg4, long jarg5, long jarg6, long jarg7, long jarg8, long jarg9); + public final static native long CanTalonSRX_GetFault_OverTemp(long jarg1, CanTalonSRX jarg1_, long jarg2);