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);