class ForwardPathFollower(Command):
    """
    This command will call the path which will go forward. The pickle file should have been
    created on the PC and placed into the autonomous folder to be uploaded with the robot code.
    """
    def __init__(self, robot):
        super().__init__()
        self.requires(robot.driveTrain)
        # self.setInterruptible(False)

        # Create references to the robot
        self.robot = robot

        # Read up the pickled path file
        with open(
                os.path.join(os.path.dirname(__file__), 'forward_path.pickle'),
                "rb") as fp:
            self.path = pickle.load(fp)

        # Control variables
        self.finished = True
        self._streamRate = int(self.path['left'][0][2] / 2)

    def isFinished(self):
        """
        This returns whether or not this command is complete.
        """
        return self.finished

    def initialize(self):
        """
        Create the left and right path follower controller objects and start the process of the
        following the path.
        """
        self.finished = False
        self.robot.driveTrain.initiaizeDrivetrainMotionProfileControllers(
            self._streamRate)
        self.pathFollower = DrivetrainMPController(
            self.robot.driveTrain.leftTalon, self.path['left'],
            self.robot.driveTrain.rightTalon, self.path['right'],
            self.robot.driveTrain.MP_SLOT0_SELECT,
            self.robot.driveTrain.MP_SLOT1_SELECT)
        self.pathFollower.start()

    def execute(self):
        """
        If the path followers has finished following the path, mark this command as complete.
        Otherwise, call the control method to update and act upon the controllers state machine.
        """
        if self.pathFollower.isFinished():
            self.finished = True
        else:
            self.pathFollower.control(self.robot.timer.get(),
                                      self.robot.smartDashboard)

    def end(self):
        '''
        Exit the DrivetrainMotionProfileControllers
        '''
        self.robot.driveTrain.cleanUpDrivetrainMotionProfileControllers()
 def initialize(self):
     """
     Create the left and right path follower controller objects and start the process of the
     following the path.
     """
     self.finished = False
     self.robot.driveTrain.initiaizeDrivetrainMotionProfileControllers(self._streamRate)
     self.pathFollower = DrivetrainMPController(self.robot.driveTrain.leftTalon,
                                                self.path['left'],
                                                self.robot.driveTrain.rightTalon,
                                                self.path['right'],
                                                self.robot.driveTrain.MP_SLOT0_SELECT,
                                                self.robot.driveTrain.MP_SLOT1_SELECT)
     self.pathFollower.start()
class DrivetrainPathFollower(Command):
    """
    This command will call the path which will go forward. The pickle file should have been created on the PC and placed into the autonomous folder
    to be uploaded with the robot code.
    """
    def __init__(self, robot, path, reverse, pid_kludge=False):
        super().__init__()
        self.requires(robot.driveTrain)

        # Create references to the robot and the path to follow
        self.robot = robot
        self.path = path
        self.reverse = reverse
        self.pidKludge = pid_kludge

        # Control variables
        self.finished = True

        # 4th value in MP's is sample period.  Assume the left and right sides are the same.  The
        # divide by 2 value is used to set the Talon control frames and notifier to twice the rate
        # of the trajectory duration.
        self._streamRate = int(self.path['left'][0][3] / 2)

    def isFinished(self):
        """
        This returns whether or not this command is complete.
        """
        return self.finished

    def initialize(self):
        """
        Create the left and right path follower controller objects and start the process of the
        following the path.
        """
        self.finished = False
        self.robot.driveTrain.initiaizeDrivetrainMotionProfileControllers(
            self._streamRate)
        if self.pidKludge:
            self.robot.driveTrain.pidKludge()
        self.pathFollower = DrivetrainMPController(
            self.robot.driveTrain.leftTalon, self.path['left'],
            self.robot.driveTrain.rightTalon, self.path['right'], self.reverse,
            self.robot.driveTrain.MP_SLOT0_SELECT,
            self.robot.driveTrain.MP_SLOT1_SELECT)
        self.pathFollower.start()

    def execute(self):
        """
        If the path followers has finished following the path, mark this command as complete.
        Otherwise, call the control method to update and act upon the controllers state machine.
        """
        if self.pathFollower.isFinished():
            self.finished = True
        else:
            self.pathFollower.control(self.robot.timer.get(),
                                      self.robot.smartDashboard)

    def end(self):
        '''
        Exit the DrivetrainMotionProfileControllers
        '''
        self.robot.driveTrain.cleanUpDrivetrainMotionProfileControllers()