class PathFollower:
def __init__(self,
left_trajectory: str,
right_trajectory: str,
ticks_per_rev: int,
wheel_diam: int,
pidva: tuple,
setter,
get_enc,
get_angle,
reverse=False,
left_offset: int = 0,
right_offset: int = 0):
self.setter = setter
self.get_angle = get_angle
self.get_enc = get_enc
self.l_output = 0
self.r_output = 0
self.finished = False
self.reverse = reverse
self.notifier = Notifier(self._path_iteration)
"""
Notice! Sides are intentionally flipped due to a bug on PathWeaver in 2019.
If you use another software or manually create paths, uncomment the following lines,
and comment out the lines for flipping paths
"""
# self.left_traectory = pathfinder.deserialize_csv(left_trajectory)
# self.right_trajectory = pathfinder.deserialize_csv(right_trajectory)
# TODO fix this after pathweaver bug fix in 2020
self.right_trajectory = pathfinder.deserialize_csv(left_trajectory)
self.left_trajectory = pathfinder.deserialize_csv(right_trajectory)
if self.reverse:
temp_right = self.right_trajectory
self.right_trajectory = self.left_trajectory
self.left_trajectory = temp_right
self.left_follower = EncoderFollower(self.left_trajectory)
self.right_follower = EncoderFollower(self.right_trajectory)
self.left_follower.configureEncoder(int(left_offset),
int(ticks_per_rev),
int(wheel_diam))
self.right_follower.configureEncoder(int(right_offset),
int(ticks_per_rev),
int(wheel_diam))
kp, ki, kd, kv, ka = pidva
self.left_follower.configurePIDVA(kp, ki, kd, kv, ka)
self.right_follower.configurePIDVA(kp, ki, kd, kv, ka)
def update_outputs(self, l_enc: int, r_enc: int, angle: float):
self.l_output = -self.left_follower.calculate(l_enc)
self.r_output = self.right_follower.calculate(r_enc)
if self.reverse:
self.l_output = self.left_follower.calculate(-l_enc)
self.r_output = -self.right_follower.calculate(-r_enc)
if self.reverse:
angle *= -1
desired_heading = pathfinder.r2d(self.left_follower.getHeading())
angle_diff = pathfinder.boundHalfDegrees(desired_heading - angle)
angle_diff = angle_diff % 360
if abs(angle_diff) > 180:
if angle_diff > 0:
angle_diff = angle_diff - 360
else:
angle_diff = angle_diff + 360
turn = 0.8 * (1.0 / 80.0) * angle_diff
if not self.reverse:
self.l_output -= turn
self.r_output -= turn
else:
self.l_output += turn
self.r_output += turn
def _path_iteration(self):
l, r = self.get_enc()
angle = self.get_angle()
self.update_outputs(l, r, angle)
self.setter(self.l_output, self.r_output)
if self.is_finished():
self.notifier.stop()
def start_following(self):
self.notifier.startPeriodic(self.left_trajectory[0].dt)
def is_finished(self):
return self.left_follower.isFinished(
) and self.right_follower.isFinished()
class DrivetrainMPController():
"""
This controller is is used to manage the interface when runing Motion Profiles on the TalonSRX
motor controllers.
This code is based off of the following java example:
https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/tree/master/Java/MotionProfiles
"""
NOTIFIER_DEBUG_CNT = 100
MIN_NUM_POINTS = 5
NUM_LOOPS_TIMEOUT = 15
def __init__(self, left_talon, left_points, right_talon, right_points, reverse, profile_slot_select0, profile_slot_select1):
# Reference to the motion profile to run
self._leftPoints = left_points
self._rightPoints = right_points
self._profileSlotSelect0 = profile_slot_select0
self._profileSlotSelect1 = profile_slot_select1
self.reverse = reverse
# Reference to the Talon SRX being used
self._leftTalon = left_talon
self._rightTalon = right_talon
# Create objects to receive the status. This way, the status will be saved in a nice named tuple.
self._leftStatus = MotionProfileStatus
self._rightStatus = MotionProfileStatus
# Control variables
self._start = False
self._state = 0
self._finished = True
self._loopTimeout = -1
self._debugCnt = self.NOTIFIER_DEBUG_CNT
# Create a _notifier to stream trajectory points into the talon. If the input stream_rate_ms is greater than 40ms, then it would be better
# to call streamMotionProfileBuffer() in the teleop/autonomous loops since we will stream at twice the rate of the motion profile...assuming
# all of the motion profile points match the first.
self._streamRateMS = int(left_points[0][3] / 2) # MP Duration
self._notifier = Notifier(self._streamToMotionProfileBuffer)
def start(self):
"""
This method is called by a command to begin execution of the motion profile.
"""
self._initialize()
self._start = True
self._state = 0
self._finished = False
self._loopTimeout = -1
self._debugCnt = self.NOTIFIER_DEBUG_CNT
def isFinished(self):
"""
This method is called by a command to know when this controller is finished.
"""
return self._finished
def control(self, time_stamp, smart_dashboard):
"""
This method is called by the command every 20ms from autonomous or teleop.
"""
# Get the current status of the motion profiler controller
self._leftStatus = self._leftTalon.getMotionProfileStatus()
self._rightStatus = self._rightTalon.getMotionProfileStatus()
# In this state, we are waiting for the start signal. Once received, make sure the Talon MPE is in a state to begin executing a new
# motion profile. The talon's should already be in a good start from the _initialize() method.
if self._state == 0:
if self._start:
# Make sure the Talon MPE is disabled before servicing the start signal
if self._leftStatus.outputEnable != SetValueMotionProfile.Disable:
logger.warning("Disabling the Left Talon MPE")
self._leftTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Disable)
elif self._rightStatus.outputEnable != SetValueMotionProfile.Disable:
logger.warning("Disabling the Right Talon MPE")
self._rightTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Disable)
# Log any prior underrun conitions
elif self._leftStatus.hasUnderrun:
logger.warning("Clearing Left Talon MPE underrun")
self._leftTalon.clearMotionProfileHasUnderrun(0)
elif self._rightStatus.hasUnderrun:
logger.warning("Clearing Right Talon MPE underrun")
self._rightTalon.clearMotionProfileHasUnderrun(0)
# Make sure the top and bottom buffers are empty
elif self._leftStatus.btmBufferCnt != 0 or self._leftStatus.topBufferCnt != 0:
logger.warning("Clearing Left Talon MPE buffer(s)")
self._leftTalon.clearMotionProfileTrajectories()
elif self._rightStatus.btmBufferCnt != 0 or self._rightStatus.topBufferCnt != 0:
logger.warning("Clearing Left Talon MPE buffer(s)")
self._rightTalon.clearMotionProfileTrajectories()
# The Talon MPE status is in a good state, start filling the top buffer and kick off the notifier which moves the top buffer data
# into the bottom buffer.
else:
logger.info("Starting the Motion Profile Controller")
self._start = False
self._state = 1
self._loopTimeout = self.NUM_LOOPS_TIMEOUT
self._startFilling()
self._notifier.startPeriodic(self._streamRateMS / 1000)
# In this state, the Talon MPE has started filling the buffer. Once enough points have been loaded into the bottom buffer, enable the
# Talon MPE.
elif self._state == 1:
if self._leftStatus.btmBufferCnt > self.MIN_NUM_POINTS and self._rightStatus.btmBufferCnt > self.MIN_NUM_POINTS:
logger.info("Talon MPE bottom buffer is ready, enabling the Talon MPE")
self._loopTimeout = self.NUM_LOOPS_TIMEOUT
self._leftTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Enable)
self._rightTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Enable)
self._state = 2
# In this state, check status of the MP and if there isn't an underrun condition, reset the loop timeout. This is basically waiting for the
# motion profile executer to complete processing the trajectories.
elif self._state == 2:
if not self._leftStatus.isUnderrun and not self._rightStatus.isUnderrun:
self._loopTimeout = self.NUM_LOOPS_TIMEOUT
else:
self._outputStatus()
# If both of the Talon's are at their last trajectory points then stop the notifier, disable the Motion Profile Executer and move on to
# state 3.
if self._leftStatus.activePointValid and self._leftStatus.isLast and self._rightStatus.activePointValid and self._rightStatus.isLast:
logger.info("Talon MPEs are at the last trajectory point")
self._notifier.stop()
self._leftTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Disable)
self._rightTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Disable)
self._state = 3
# Output debug data to the smartdashboard. This will include all of the data needed to dig into the closed loop motion profile.
if LOGGER_LEVEL == logging.DEBUG:
self._outputData(smart_dashboard, time_stamp)
# In this state, we are ready to exit the motion profile. Mark the command as complete and remove the notifier.
elif self._state == 3:
logger.info("Stopping the Motion Profile Controller")
self._finished = True
# Call the free method on the notifier to hopefully stop/destroy it. Dereference the notifier object...not sure if the garbage collector
# will do the rest. This needs Chris's help.
self._notifier.free()
del(self._notifier)
# Service the loop timeout. If the loop is stalled out, report the motion profile status, set the state to 3, and stop the notifier. This
# will hopefully exit gracefully.
if self._loopTimeout < 0:
pass
else:
if self._loopTimeout == 0:
logger.warning("No progress being made - State = %i" % (self._state))
self._outputStatus()
self._state = 3
self._notifier.stop()
else:
self._loopTimeout -= 1
def _initialize(self):
"""
This method will initialize the motion profile controller by clearing out any trajectories still in the buffer, setting the control mode to
motion profile (disabled), change the control frame period to the stream rate, and clearing any prior buffer underruns.
"""
self._leftTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Disable)
self._rightTalon.set(WPI_TalonSRX.ControlMode.MotionProfileArc, SetValueMotionProfile.Disable)
if self._leftStatus.hasUnderrun:
logger.warning("Clearing Left Talon UNDERRUN condition during initialization")
self._leftTalon.clearMotionProfileHasUnderrun(0)
if self._rightStatus.hasUnderrun:
logger.warning("Clearing Right Talon UNDERRUN condition during initialization")
self._rightTalon.clearMotionProfileHasUnderrun(0)
if self._leftStatus.btmBufferCnt != 0 or self._leftStatus.topBufferCnt != 0:
logger.warning("Clearing Left Talon MPE buffer(s) during initialization")
self._leftTalon.clearMotionProfileTrajectories()
if self._rightStatus.btmBufferCnt != 0 or self._leftStatus.topBufferCnt != 0:
logger.warning("Clearing Right Talon MPE buffer(s) during initialization")
self._rightTalon.clearMotionProfileTrajectories()
def _streamToMotionProfileBuffer(self):
"""
This method will move the trajectory points from the top-buffer to the bottom-buffer.
"""
# Print out a message letting the us know that the notifier is running.
if self._debugCnt == 0:
if self._finished:
logger.warning('Motion Profile Controller notifier is still running')
self._debugCnt = self.NOTIFIER_DEBUG_CNT
else:
self._debugCnt -= 1
# The bottom buffer size is 128. Make sure there is space before moving trying to move the data.
if self._leftStatus.btmBufferCnt < 100:
self._leftTalon.processMotionProfileBuffer()
if self._rightStatus.btmBufferCnt < 100:
self._rightTalon.processMotionProfileBuffer()
def _startFilling(self):
"""
This method will start filling the top buffer of the Talon MPE. This will execute quickly. If the motion profile is meant to have the robot
drive backwards, then use negative postion target and negative velocity/FF. The closed loop postion values will be zero'd out at the
beginning of each path. This does not include the zero'ing of the gyro.
"""
for i in range(len(self._leftPoints)):
point = TrajectoryPoint(-self._leftPoints[i][0] if self.reverse else self._leftPoints[i][0], # Position
-self._leftPoints[i][1] if self.reverse else self._leftPoints[i][1], # Velocity / Feed-Forward
self._leftPoints[i][2], # Heading
self._profileSlotSelect0, # PID0 slot index
self._profileSlotSelect1, # PID0 slot index
True if i+1 == len(self._leftPoints) else False, # Last point flag
True if i == 0 else False, # Zero postion flag
self._getTrajectoryDuration(self._leftPoints[i][3])) # Duration
self._leftTalon.pushMotionProfileTrajectory(point) # Push the trajectory point (top buffer)
point = TrajectoryPoint(-self._rightPoints[i][0] if self.reverse else self._rightPoints[i][0],
-self._rightPoints[i][1] if self.reverse else self._rightPoints[i][1],
self._leftPoints[i][2],
self._profileSlotSelect0,
self._profileSlotSelect1,
True if i+1 == len(self._rightPoints) else False,
True if i == 0 else False,
self._getTrajectoryDuration(self._rightPoints[i][3]))
self._rightTalon.pushMotionProfileTrajectory(point)
def _getTrajectoryDuration(self, duration):
"""
This method will return the trajectory duration enumeration.
"""
if duration == 0:
return TrajectoryPoint.TrajectoryDuration.T0ms
elif duration == 5:
return TrajectoryPoint.TrajectoryDuration.T5ms
elif duration == 10:
return TrajectoryPoint.TrajectoryDuration.T10ms
elif duration == 20:
return TrajectoryPoint.TrajectoryDuration.T20ms
elif duration == 30:
return TrajectoryPoint.TrajectoryDuration.T30ms
elif duration == 40:
return TrajectoryPoint.TrajectoryDuration.T40ms
elif duration == 50:
return TrajectoryPoint.TrajectoryDuration.T50ms
elif duration == 100:
return TrajectoryPoint.TrajectoryDuration.T100ms
else:
logger.warning("Unsupported duration %i" % (duration))
return TrajectoryPoint.TrajectoryDuration.T0ms
def _outputStatus(self):
logger.warning("LEFT: isUnderrun: %s, hasUnderrun: %s, topBufferRem: %s, topBufferCnt: %i, btmBufferCnt: %i, activePointValid: %s, "
" isLast: %s, mode: %i, timeDureMS: %i" %
(self._leftStatus.isUnderrun, self._leftStatus.hasUnderrun, self._leftStatus.topBufferRem, self._leftStatus.topBufferCnt,
self._leftStatus.btmBufferCnt, self._leftStatus.activePointValid, self._leftStatus.isLast, self._leftStatus.outputEnable,
self._leftStatus.timeDurMs))
logger.warning("RIGHT: isUnderrun: %s, hasUnderrun: %s, topBufferRem: %s, topBufferCnt: %i, btmBufferCnt: %i, activePointValid: %s, "
"isLast: %s, mode: %i, timeDureMS: %i" %
(self._rightStatus.isUnderrun, self._rightStatus.hasUnderrun, self._rightStatus.topBufferRem, self._rightStatus.topBufferCnt,
self._rightStatus.btmBufferCnt, self._rightStatus.activePointValid, self._rightStatus.isLast, self._rightStatus.outputEnable,
self._rightStatus.timeDurMs))
def _outputData(self, smart_dashboard, time_stamp):
smart_dashboard.putNumber("RightEncPos", self._rightTalon.getSensorCollection().getQuadraturePosition())
smart_dashboard.putNumber("RightActPos", self._rightTalon.getActiveTrajectoryPosition())
smart_dashboard.putNumber("RightEncVel", self._rightTalon.getAnalogInVel())
smart_dashboard.putNumber("RightActVel", self._rightTalon.getActiveTrajectoryVelocity())
smart_dashboard.putNumber("RightPrimaryError", self._rightTalon.getClosedLoopError(0))
smart_dashboard.putNumber("RightSecondaryError", self._rightTalon.getClosedLoopError(1))
smart_dashboard.putNumber("LeftEncPos", self._leftTalon.getSensorCollection().getQuadraturePosition())
smart_dashboard.putNumber("LeftActPos", self._leftTalon.getActiveTrajectoryPosition())
smart_dashboard.putNumber("LeftEncVel", self._leftTalon.getAnalogInVel())
smart_dashboard.putNumber("LeftActVel", self._leftTalon.getActiveTrajectoryVelocity())
smart_dashboard.putNumber("LeftPrimaryError", self._leftTalon.getClosedLoopError(0))
smart_dashboard.putNumber("LeftSecondaryError", self._leftTalon.getClosedLoopError(1))
smart_dashboard.putNumber("RightTopBufferCount", self._rightStatus.topBufferCnt)
smart_dashboard.putNumber("LeftTopBufferCount", self._leftStatus.topBufferCnt)
smart_dashboard.putNumber("LeftBottomBufferCount", self._leftStatus.btmBufferCnt)
smart_dashboard.putNumber("RightBottomBufferCount", self._rightStatus.btmBufferCnt)
smart_dashboard.putNumber("TimeStamp", time_stamp)
class MotionProfileController():
"""
This controller is is used to manage the interface when runing Motion Profiles on the TalonSRX
motor controllers.
This code is based off of the following java example:
https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/tree/master/Java/MotionProfiles
"""
NOTIFIER_DEBUG_CNT = 100
MIN_NUM_POINTS = 5
NUM_LOOPS_TIMEOUT = 10
def __init__(self, talon, points, reverse, starting_position,
profile_slot_select0, profile_slot_select1):
# Reference to the motion profile to run
self._points = points
self._profileSlotSelect0 = profile_slot_select0
self._profileSlotSelect1 = profile_slot_select1
self.reverse = reverse
self.startingPostion = starting_position
# Reference to the Talon SRX being used
self._talon = talon
# Reference used to collect the status of the named tuple data transfer object
self._status = MotionProfileStatus
# Reference used to collect the trajectory point of the named tuple data transfer object
self._point = TrajectoryPoint
# Control variables
self._start = False
self._state = 0
self._finished = True
self._loopTimeout = -1
self._debugCnt = self.NOTIFIER_DEBUG_CNT
# Create a _notifier to stream trajectory points into the talon. If the input
# stream_rate_ms is greater than 40ms, then it would be better to call
# streamMotionProfileBuffer() in the teleop/autonomous loops since we will stream at twice
# the rate of the motion profile
self._streamRateMS = int(points[0][3] / 2)
self._notifier = Notifier(self._streamToMotionProfileBuffer)
def start(self):
"""
This method is called by a command to begin execution of the motion profile.
"""
self._initialize()
self._start = True
self._state = 0
self._finished = False
self._loopTimeout = -1
self._debugCnt = self.NOTIFIER_DEBUG_CNT
def isFinished(self):
"""
This method is called by a command to know when the path follower is finished.
"""
return self._finished
def control(self):
"""
This method is called by the command every 20ms in autonomous or teleop.
"""
# Get the current _status
self._status = self._talon.getMotionProfileStatus()
# Waiting for the start signal. Once received, make sure the Talon MPE is in a state to
# begin executing a new motion profile.
if self._state == 0:
if self._start:
# Make sure the Talon MPE is disabled before servicing the start signal
if self._status.outputEnable != SetValueMotionProfile.Disable:
logger.warning("Disabling the Talon MPE")
self._talon.set(WPI_TalonSRX.ControlMode.MotionProfile,
SetValueMotionProfile.Disable)
# Log any prior underrun conitions
elif self._status.hasUnderrun:
logger.warning("Clearing Talon MPE underrun")
self._talon.clearMotionProfileHasUnderrun(0)
# Make sure the top and bottom buffers are empty
elif self._status.btmBufferCnt != 0 or self._status.topBufferCnt != 0:
logger.warning("Clearing Talon MPE buffer(s)")
self._talon.clearMotionProfileTrajectories()
# The Talon MPE status is in a good state, start filling the top buffer and kick
# off the notifier which moves the top buffer data into the bottom buffer.
else:
logger.info("Starting the Motion Profile Controller")
self._start = False
self._state = 1
self._loopTimeout = self.NUM_LOOPS_TIMEOUT
self._startFilling()
self._notifier.startPeriodic(self._streamRateMS / 1000)
logger.info("Started Motion Profile Controller _notifier")
# The Talon MPE has started filling the buffer. Once enough points have been loaded into
# the bottom buffer, enable the Talon MPE.
elif self._state == 1:
if self._status.btmBufferCnt > self.MIN_NUM_POINTS:
logger.info(
"Talon MPE bottom buffer is ready, enabling the Talon MPE")
self._state = 2
self._loopTimeout = self.NUM_LOOPS_TIMEOUT
self._talon.set(WPI_TalonSRX.ControlMode.MotionProfile,
SetValueMotionProfile.Enable)
# Check status of the MP and if there isn't an underrun condition, reset the loop timeout.
# Once the last point has been processed, stop the notifier and set the Talon MPE to hold
# the final position.
# ***** TODO ***** (may want to go to neutral instead)
elif self._state == 2:
if not self._status.isUnderrun:
self._loopTimeout = self.NUM_LOOPS_TIMEOUT
else:
self._outputStatus()
if self._status.activePointValid and self._status.isLast:
logger.info("Talon MPE is at the last trajectory point")
self._state = 3
self._notifier.stop()
logger.info(
"Called to stop Motion Profile Controller notifier")
self._talon.set(WPI_TalonSRX.ControlMode.MotionProfile,
SetValueMotionProfile.Disable)
elif self._state == 3:
self._state = 0
self._loopTimeout -= 1
self._finished = True
# Remove the reference to the notifier and hopefully the notifier will be stopped
self._notifier.free()
del (self._notifier)
# Service the loop timeout
if self._loopTimeout < 0:
pass
else:
if self._loopTimeout == 0:
logger.warning("No progress being made - State = %i" %
(self._state))
self._outputStatus()
self._state = 3
self._notifier.stop()
else:
self._loopTimeout -= 1
def _initialize(self):
"""
This method will initialize the motion profile controller by clearing out any trajectories
still in the buffer, setting the control mode to motion profile (disabled), change the
control frame period to the stream rate, and clearing any prior buffer underruns.
"""
self._talon.changeMotionControlFramePeriod(self._streamRateMS)
self._talon.set(WPI_TalonSRX.ControlMode.MotionProfile,
SetValueMotionProfile.Disable)
if self._status.hasUnderrun:
logger.warning("Clearing UNDERRUN condition during reset")
self._talon.clearMotionProfileHasUnderrun(0)
if self._status.btmBufferCnt != 0 or self._status.topBufferCnt != 0:
logger.warning("Clearing MPE buffer(s)")
self._talon.clearMotionProfileTrajectories()
def _streamToMotionProfileBuffer(self):
"""
This method will move the trajectory points from the top-buffer to the bottom-buffer.
"""
if self._debugCnt == 0:
if self._finished:
logger.warning('Motion Profile Controller notifier is running')
self._debugCnt = self.NOTIFIER_DEBUG_CNT
else:
self._debugCnt -= 1
if self._status.btmBufferCnt < 100:
self._talon.processMotionProfileBuffer()
#=======================================================================================
# logger.info("leftbtmBufferCnt: %i, rightbtmBufferCnt: %i," %
# (self._leftStatus.btmBufferCnt, self._rightStatus.btmBufferCnt))
#=======================================================================================
def _getTrajectoryDuration(self, duration):
"""
This method will return the trajectory duration enumeration.
"""
if duration == 0:
return TrajectoryPoint.TrajectoryDuration.T0ms
elif duration == 5:
return TrajectoryPoint.TrajectoryDuration.T5ms
elif duration == 10:
return TrajectoryPoint.TrajectoryDuration.T10ms
elif duration == 20:
return TrajectoryPoint.TrajectoryDuration.T20ms
elif duration == 30:
return TrajectoryPoint.TrajectoryDuration.T30ms
elif duration == 40:
return TrajectoryPoint.TrajectoryDuration.T40ms
elif duration == 50:
return TrajectoryPoint.TrajectoryDuration.T50ms
elif duration == 100:
return TrajectoryPoint.TrajectoryDuration.T100ms
else:
logger.critical("Unsupported duration %i" % (duration))
return TrajectoryPoint.TrajectoryDuration.T0ms
def _startFilling(self):
"""
This method will start filling the top buffer of the Talon MPE. This will execute quickly.
"""
for i in range(len(self._points)):
if self.reverse:
self._point.position = -self._points[i][0]
self._point.velocity = -self._points[i][1]
else:
self._point.position = self._points[i][0]
self._point.velocity = self._points[i][1]
self._point.auxiliaryPos = self._points[i][2]
self._point.profileSlotSelect0 = self._profileSlotSelect0
self._point.profileSlotSelect1 = self._profileSlotSelect1
self._point.timeDur = self._getTrajectoryDuration(
self._points[i][3])
self._point.zeroPos = False
if i == 0:
self._point.zeroPos = True
self._point.isLastPoint = False
if i + 1 == len(self._points):
self._point.isLastPoint = True
self._talon.pushMotionProfileTrajectory(self._point)
def _outputStatus(self):
logger.warning(
"isUnderrun: %s, hasUnderrun: %s, topBufferRem: %s, topBufferCnt: %i, "
"btmBufferCnt: %i, activePointValid: %s, isLast: %s" %
(self._status.isUnderrun, self._status.hasUnderrun,
self._status.topBufferRem, self._status.topBufferCnt,
self._status.btmBufferCnt, self._status.activePointValid,
self._status.isLast))