/**
* push caller's uncompressed simple trajectory point.
*/
public void Push(TALON_Control_6_MotProfAddTrajPoint_huff0_t pt)
{
Alloc();
_motProfTopBuffer[_in] = pt;
if (++_in >= _cap)
_in = 0;
++_sz;
}
/**
* push caller's uncompressed simple trajectory point.
*/
public void Push(TALON_Control_6_MotProfAddTrajPoint_huff0_t pt)
{
Alloc();
_motProfTopBuffer[_in] = pt;
if (++_in >= _cap)
{
_in = 0;
}
++_sz;
}
/**
* Push another trajectory point into the top level buffer (which is emptied into the Talon's
* bottom buffer as room allows).
*
* @param targPos servo position in native Talon units (sensor units).
* @param targVel velocity to feed-forward in native Talon units (sensor units per
* 100ms).
* @param profileSlotSelect which slot to pull PIDF gains from. Currently supports 0 or 1.
* @param timeDurMs time in milliseconds of how long to apply this point.
* @param velOnly set to nonzero to signal Talon that only the feed-foward velocity
* should be used, i.e. do not perform PID on position. This is
* equivalent to setting PID gains to zero, but much more efficient
* and synchronized to MP.
* @param isLastPoint set to nonzero to signal Talon to keep processing this trajectory
* point, instead of jumping to the next one when timeDurMs expires.
* Otherwise MP executer will eventually see an empty buffer after
* the last point expires, causing it to assert the IsUnderRun flag.
* However this may be desired if calling application never wants to
* terminate the MP.
* @param zeroPos set to nonzero to signal Talon to "zero" the selected position
* sensor before executing this trajectory point. Typically the
* first point should have this set only thus allowing the remainder
* of the MP positions to be relative to zero.
*
* @return CTR_OKAY if trajectory point push ok. CTR_BufferFull if buffer is full due to
* kMotionProfileTopBufferCapacity.
*/
public ErrorCode PushMotionProfileTrajectory(
double position, double velocity, double turn,
int profileSlotSelect0, int profileSlotSelect1, bool isLastPoint, bool zeroPos, int durationMs)
{
ErrorCode retval = ErrorCode.OK;
int turnUnits = (int)(turn);
int targPos = (int)position;
int targVel = (int)velocity;
/* sterilize and check inputs */
if (profileSlotSelect0 < 0)
{
profileSlotSelect0 = 0;
retval = ErrorCode.InvalidParamValue;
}
if (profileSlotSelect0 > 3)
{
profileSlotSelect0 = 3;
retval = ErrorCode.InvalidParamValue;
}
if (profileSlotSelect1 < 0)
{
profileSlotSelect1 = 0;
retval = ErrorCode.InvalidParamValue;
}
if (profileSlotSelect1 > 1)
{
profileSlotSelect1 = 1;
retval = ErrorCode.InvalidParamValue;
}
byte b0 = (byte)profileSlotSelect0;
b0 <<= 1;
b0 |= (byte)(zeroPos ? 1 : 0);
b0 <<= 3;
b0 |= (byte)DurationMsToEnum(durationMs);
byte headingH = (byte)((turnUnits >> 8) & 0x3F);
byte headingL = (byte)(turnUnits & 0xFF);
byte b1 = (byte)(isLastPoint ? 1 : 0);
b1 <<= 1;
b1 |= (byte)profileSlotSelect1;
b1 <<= 6;
b1 |= headingH;
byte b2 = headingL;
byte b3 = (byte)(targVel >> 0x08);
byte b4 = (byte)(targVel & 0xFF);
byte b5 = (byte)(targPos >> 0x10);
byte b6 = (byte)(targPos >> 0x08);
byte b7 = (byte)(targPos & 0xFF);
TALON_Control_6_MotProfAddTrajPoint_huff0_t traj = 0;
traj |= b7;
traj <<= 8;
traj |= b6;
traj <<= 8;
traj |= b5;
traj <<= 8;
traj |= b4;
traj <<= 8;
traj |= b3;
traj <<= 8;
traj |= b2;
traj <<= 8;
traj |= b1;
traj <<= 8;
traj |= b0;
lock (_mutMotProf)
{
if (_motProfTopBuffer.GetNumTrajectories() >= kMotionProfileTopBufferCapacity)
{
return(Reporting.Log(ErrorCode.BufferFull, ToString(), 0, ""));
}
_motProfTopBuffer.Push(traj);
}
if (retval != ErrorCode.OK)
{
return(Reporting.Log(retval, ToString(), 0, ""));
}
return(ErrorCode.OK);
}
