001package com.ctre.phoenix.motion;
002
003import com.ctre.phoenix.ErrorCode;
004import com.ctre.phoenix.motion.TrajectoryPoint;
005import com.ctre.phoenix.motion.BuffTrajPointStreamJNI;
006
007/**
008 * Stream of trajectory points for Talon/Victor motion profiling.
009 */
010public class BufferedTrajectoryPointStream{
011    private long m_handle;
012
013    public BufferedTrajectoryPointStream()
014    {
015        m_handle = BuffTrajPointStreamJNI.Create1();
016    }
017        /**
018         * Clear all trajectory points.
019         * @return nonzero error code if operation fails.
020         */
021    public ErrorCode Clear()
022    {
023        return ErrorCode.valueOf(BuffTrajPointStreamJNI.Clear(m_handle));
024    }
025        /**
026         * Write a single trajectory point into the buffer.
027         * @param trajPt        Trajectory point to write.
028         * @return nonzero error code if operation fails.
029         */
030    public ErrorCode Write(TrajectoryPoint trajPt)
031    {
032        int status = BuffTrajPointStreamJNI.Write(m_handle,
033            trajPt.position,
034            trajPt.velocity,
035            trajPt.arbFeedFwd,
036            trajPt.auxiliaryPos,
037            trajPt.auxiliaryVel,
038            trajPt.auxiliaryArbFeedFwd,
039            trajPt.profileSlotSelect0,
040            trajPt.profileSlotSelect1,
041            trajPt.isLastPoint,
042            trajPt.zeroPos,
043            trajPt.timeDur,
044            trajPt.useAuxPID);
045        return ErrorCode.valueOf(status);
046    }
047        /**
048         * Writes an array of trajectory point into the buffer.
049         * @param trajPts       Array of trajectory points to write.
050         * @param trajPtCount  Number of points to write.  This is capped against array length.
051         * @return nonzero error code if operation fails.
052         */
053    public ErrorCode Write(TrajectoryPoint[] trajPts, int trajPtCount)
054    {
055        ErrorCode retval = ErrorCode.OK;
056
057        if(trajPtCount > trajPts.length){trajPtCount = trajPts.length;}
058
059        for (int i = 0; i < trajPtCount; ++i) {
060            /* insert next pt */
061            ErrorCode er = Write(trajPts[i]);
062            /* save first nonzero error code */
063            if (retval == ErrorCode.OK) { retval = er; }
064        }
065
066        return retval;
067    }
068        /**
069         * Writes an array of trajectory point into the buffer.
070         * @param trajPts       Array of trajectory points to write.
071         * @return nonzero error code if operation fails.
072         */
073    public ErrorCode Write(TrajectoryPoint[] trajPts){
074        return Write(trajPts, trajPts.length);
075    }
076
077    /**
078     * @return handle of object
079     */
080    public long getHandle(){return m_handle;}
081}