예제 #1
0
파일: RobotPi.py 프로젝트: radiumray/aracna
    def interpMove(self,
                   start,
                   end,
                   seconds,
                   logFile=None,
                   extraLogInfoFn=None):
        '''Moves between start and end over seconds seconds.  start
        and end may be functions of the time.'''

        self.updateClock()

        timeStart = self.time
        timeEnd = self.time + seconds

        ii = 0
        tlast = self.time
        while self.time < timeEnd:
            #print 'time:', self.time
            ii += 1
            posS = start(self.time) if isinstance(start,
                                                  FunctionType) else start
            posE = end(self.time) if isinstance(end, FunctionType) else end
            print "Start: " + str(posS)
            print "End: " + str(posE)
            goal = lInterp(self.time, [timeStart, timeEnd], posS, posE)

            #startSend = datetime.now()
            cmdPos = self.commandPosition(goal)
            #deltaTimeSend = datetime.now() - startSend
            #print "sent: " + str(deltaTimeSend) #TODO: remove this line after testing
            if logFile:
                extraInfo = ''
                if extraLogInfoFn:
                    extraInfo = extraLogInfoFn()
                print >> logFile, self.time, ' '.join([str(x)
                                                       for x in cmdPos]),
                #print >>logFile, ' '.join(str(ac.current_position) for ac in self.actuators),
                print >> logFile, extraInfo

            #volts = ['%d: %s' % (ii,ac.current_voltage) for ii,ac in enumerate(self.actuators)]
            #print ' '.join(volts)

            #[ac.read_all() for ac in self.actuators]
            #positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)]
            #print ' '.join(positions)
            #print ''.join(['x' if ac.led else ' ' for ac in self.actuators])

            #sleep(self.sleep)
            #sleep(float(1)/100)
            self.updateClock()
            secElapsed = self.time - tlast
            tosleep = self.sleep - secElapsed
            if tosleep > 0:
                sleep(tosleep)
            self.updateClock()
            tlast = self.time
예제 #2
0
파일: RobotPi.py 프로젝트: PHPDOTSQL/aracna
    def interpMove(self, start, end, seconds, logFile=None, extraLogInfoFn=None):
        '''Moves between start and end over seconds seconds.  start
        and end may be functions of the time.'''

        self.updateClock()
        
        timeStart = self.time
        timeEnd   = self.time + seconds

        ii = 0
        tlast = self.time
        while self.time < timeEnd:
            #print 'time:', self.time
            ii += 1
            posS = start(self.time) if isinstance(start, FunctionType) else start
            posE =   end(self.time) if isinstance(end,   FunctionType) else end
            print "Start: " + str(posS)
            print "End: " + str(posE)
            goal = lInterp(self.time, [timeStart, timeEnd], posS, posE)
            
            #startSend = datetime.now()
            cmdPos = self.commandPosition(goal)
            #deltaTimeSend = datetime.now() - startSend
            #print "sent: " + str(deltaTimeSend) #TODO: remove this line after testing
            if logFile:
                extraInfo = ''
                if extraLogInfoFn:
                    extraInfo = extraLogInfoFn()
                print >>logFile, self.time, ' '.join([str(x) for x in cmdPos]),
                #print >>logFile, ' '.join(str(ac.current_position) for ac in self.actuators),
                print >>logFile, extraInfo

            #volts = ['%d: %s' % (ii,ac.current_voltage) for ii,ac in enumerate(self.actuators)]
            #print ' '.join(volts)

            #[ac.read_all() for ac in self.actuators]
            #positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)]
            #print ' '.join(positions)
            #print ''.join(['x' if ac.led else ' ' for ac in self.actuators])
                
            #sleep(self.sleep)
            #sleep(float(1)/100)
            self.updateClock()
            secElapsed = self.time - tlast
            tosleep = self.sleep - secElapsed
            if tosleep > 0:
                sleep(tosleep)
            self.updateClock()
            tlast = self.time
예제 #3
0
def repeating_motion(time, intervals, poses):
    '''Return position assumign repeated motion between the given
    poses where each segment takes a length of time given by
    intervals.
    '''

    assert len(intervals) == len(poses)
    time_points = cumsum([0] + intervals)
    total_duration = time_points[-1]
    relative_time = time % total_duration
    for ii in xrange(len(time_points) - 1):
        if (relative_time < time_points[ii + 1]):
            return lInterp(relative_time,
                           [time_points[ii], time_points[ii + 1]], poses[ii],
                           poses[(ii + 1) % len(poses)])
    raise Exception('Logic error; should not get here')
예제 #4
0
def repeating_motion(time, intervals, poses):
    '''Return position assumign repeated motion between the given
    poses where each segment takes a length of time given by
    intervals.
    '''
    
    assert len(intervals) == len(poses)
    time_points = cumsum([0] + intervals)
    total_duration = time_points[-1]
    relative_time = time % total_duration
    for ii in xrange(len(time_points) - 1):
        if (relative_time < time_points[ii+1]):
            return lInterp(relative_time,
                           [time_points[ii], time_points[ii+1]],
                           poses[ii],
                           poses[(ii+1)%len(poses)])
    raise Exception('Logic error; should not get here')