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