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)
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))