def clockwiseSpin():
	ref = dd.H_REF()
	buff = pack(1,1,1023)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,0,1023)
	ref = ser.serial_sim(r,ref,buff)
	
	simSleepTwentyHz(650)
	
	buff = pack(1,1,0)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,0,0)
	ref = ser.serial_sim(r,ref,buff)
def counterClockSpin():
	ref = dd.H_REF()
	buff = pack(1,0,511)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,1,511)
	ref = ser.serial_sim(r,ref,buff)
	
	simSleepTwentyHz(1300)
	
	buff = pack(1,1,0)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,0,0)
	ref = ser.serial_sim(r,ref,buff)
def counterClockSpin():
    ref = dd.H_REF()
    buff = pack(1, 0, 511)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 1, 511)
    ref = ser.serial_sim(r, ref, buff)

    simSleepTwentyHz(1300)

    buff = pack(1, 1, 0)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 0, 0)
    ref = ser.serial_sim(r, ref, buff)
def clockwiseSpin():
    ref = dd.H_REF()
    buff = pack(1, 1, 1023)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 0, 1023)
    ref = ser.serial_sim(r, ref, buff)

    simSleepTwentyHz(650)

    buff = pack(1, 1, 0)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 0, 0)
    ref = ser.serial_sim(r, ref, buff)
def square(direction):
	# direction - Counterclockwise = 1, Clockwise = 0
	ref = dd.H_REF()
	buff = pack(1,1,1023)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,1,1023)
	ref = ser.serial_sim(r,ref,buff)
	simSleepTwentyHz(140)
	c = 0
	while c < 15:
		turnNinty(direction)
		simSleepTwentyHz(140)
		c += 1

	buff = pack(1,1,0)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,1,0)
	ref = ser.serial_sim(r,ref,buff)
def square(direction):
    # direction - Counterclockwise = 1, Clockwise = 0
    ref = dd.H_REF()
    buff = pack(1, 1, 1023)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 1, 1023)
    ref = ser.serial_sim(r, ref, buff)
    simSleepTwentyHz(140)
    c = 0
    while c < 15:
        turnNinty(direction)
        simSleepTwentyHz(140)
        c += 1

    buff = pack(1, 1, 0)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 1, 0)
    ref = ser.serial_sim(r, ref, buff)
def turnNinty(direction):
	# Direction - Left = 1, Right = 0
	ref = dd.H_REF()
	if direction: # left turn
		i = 1
		while i < 10:
			buff = pack(1,1,1023/i)
			ref = ser.serial_sim(r,ref,buff)
			buff = pack(0,1,1023)
			ref = ser.serial_sim(r,ref,buff)
			simSleepTwentyHz(11)
			i += 1
		i = 7
		while i > 0:
			buff = pack(1,1,1023/i)
			ref = ser.serial_sim(r,ref,buff)
			buff = pack(0,1,1023)
			ref = ser.serial_sim(r,ref,buff)
			simSleepTwentyHz(3)
			i -= 1
	else: # right turn
		i = 1
		while i < 10:
			buff = pack(1,1,1023)
			ref = ser.serial_sim(r,ref,buff)
			buff = pack(0,1,1023/i)
			ref = ser.serial_sim(r,ref,buff)
			simSleepTwentyHz(11)
			i += 1
		i = 7
		while i > 0:
			buff = pack(1,1,1023)
			ref = ser.serial_sim(r,ref,buff)
			buff = pack(0,1,1023/i)
			ref = ser.serial_sim(r,ref,buff)
			simSleepTwentyHz(3)
			i -= 1
	# forward
	buff = pack(1,1,1023)
	ref = ser.serial_sim(r,ref,buff)
	buff = pack(0,1,1023)
	ref = ser.serial_sim(r,ref,buff)
def turnNinty(direction):
    # Direction - Left = 1, Right = 0
    ref = dd.H_REF()
    if direction:  # left turn
        i = 1
        while i < 10:
            buff = pack(1, 1, 1023 / i)
            ref = ser.serial_sim(r, ref, buff)
            buff = pack(0, 1, 1023)
            ref = ser.serial_sim(r, ref, buff)
            simSleepTwentyHz(11)
            i += 1
        i = 7
        while i > 0:
            buff = pack(1, 1, 1023 / i)
            ref = ser.serial_sim(r, ref, buff)
            buff = pack(0, 1, 1023)
            ref = ser.serial_sim(r, ref, buff)
            simSleepTwentyHz(3)
            i -= 1
    else:  # right turn
        i = 1
        while i < 10:
            buff = pack(1, 1, 1023)
            ref = ser.serial_sim(r, ref, buff)
            buff = pack(0, 1, 1023 / i)
            ref = ser.serial_sim(r, ref, buff)
            simSleepTwentyHz(11)
            i += 1
        i = 7
        while i > 0:
            buff = pack(1, 1, 1023)
            ref = ser.serial_sim(r, ref, buff)
            buff = pack(0, 1, 1023 / i)
            ref = ser.serial_sim(r, ref, buff)
            simSleepTwentyHz(3)
            i -= 1
    # forward
    buff = pack(1, 1, 1023)
    ref = ser.serial_sim(r, ref, buff)
    buff = pack(0, 1, 1023)
    ref = ser.serial_sim(r, ref, buff)
#-----------------------------------------------------
#--------[ Do not edit above ]------------------------
#-----------------------------------------------------
    # Main Loop
    # Def:
    # tim.sim[0] = Sim Time


	print 'Sim Time = ', tim.sim[0]
	prev_tim = tim.sim[0]
	# Sleeps

	#time.sleep(0.1)   
	buff = createCommandPacket(0x00, 4, 0x20, [0xFF, 0x05])
	ref = ser.serial_sim(r,ref,buff)
	buff = createCommandPacket(0x01, 4, 0x20, [0xFF, 0x01])
	ref = ser.serial_sim(r,ref,buff)


	while tim.sim[0] < prev_tim + 0.05:
		time.sleep(0.001)
		[status, framesize] = t.get(tim, wait=False, last=True)
		if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME or status == ach.ACH_STALE_FRAMES:
			pass
			#print 'Sim Time = ', tim.sim[0]
		else:
			raise ach.AchException( v.result_string(status) )


#-----------------------------------------------------
        raise ach.AchException(v.result_string(status))

#-----------------------------------------------------
#--------[ Do not edit above ]------------------------
#-----------------------------------------------------
# Main Loop
# Def:
# tim.sim[0] = Sim Time

    print 'Sim Time = ', tim.sim[0]
    prev_tim = tim.sim[0]
    # Sleeps

    #time.sleep(0.1)
    buff = createCommandPacket(0x00, 4, 0x20, [0xFF, 0x05])
    ref = ser.serial_sim(r, ref, buff)
    buff = createCommandPacket(0x01, 4, 0x20, [0xFF, 0x01])
    ref = ser.serial_sim(r, ref, buff)

    while tim.sim[0] < prev_tim + 0.05:
        time.sleep(0.001)
        [status, framesize] = t.get(tim, wait=False, last=True)
        if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME or status == ach.ACH_STALE_FRAMES:
            pass
            #print 'Sim Time = ', tim.sim[0]
        else:
            raise ach.AchException(v.result_string(status))

#-----------------------------------------------------
#--------[ Do not edit below ]------------------------
#-----------------------------------------------------
		#print 'Sim Time = ', tim.sim[0]
	else:
		raise ach.AchException( v.result_string(status) )
	timeStart = tim.sim[0]
	
	#Main loop

	#grab axis values from joystick channel
	[statusj, framesizej] = j.get(jstate, wait=False, last=False)
	#for i in range(0, 6):
		#print 'Axis ', i, ' = ', jstate.axis[i]
	if(controlMode == "simple"):
		buff = simpleMove(jstate)
	elif(controlMode == "idcs"):
		buff = intelligentMove(jstate)
	ref = ser.serial_sim(r, ref, buff[0])
	ref = ser.serial_sim(r, ref, buff[1])
	

	#sleep until the update period as passed
	[status, framesize] = t.get(tim, wait=False, last=True)
	if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME or status == ach.ACH_STALE_FRAMES:
		pass
		#print 'Sim Time = ', tim.sim[0]
	else:
		raise ach.AchException( v.result_string(status) )
#	time.sleep(updatePeriod - (tim.sim[0]- timeStart) )
	time.sleep(updatePeriod)


Exemplo n.º 12
0
import ach
import sys
import time
import numpy as np
from AX12 import *
import diff_drive
import socket
import actuator_sim as ser

dd = diff_drive
ref = dd.H_REF()
tim = dd.H_TIME()

ROBOT_DIFF_DRIVE_CHAN = 'robot-diff-drive'
ROBOT_CHAN_VIEW = 'robot-vid-chan'
ROBOT_TIME_CHAN = 'robot-time'

r = ach.Channel(ROBOT_DIFF_DRIVE_CHAN)
r.flush()
t = ach.Channel(ROBOT_TIME_CHAN)
t.flush()
c = ach.Channel(ci.CONTROLLER_REF_NAME)
c.flush()
controller = ci.CONTROLLER_REF()
c.put(controller)

while True:
  [statuss, framesizes] = c.get(controller, wait=False, last= False)
  ref = ser.serial_sim(r,ref,speedcmd(0,controller.mot1))
  ref = ser.serial_sim(r,ref,speedcmd(1,controller.mot2))