def simpleMove(jstate): leftWheel = 0x01 rightWheel = 0x00 leftAxis = -1.0 * jstate.axis[1] #Up on joystick give -1, so make up positive rightAxis = -1.0 * jstate.axis[3] fullSpeedAhead = [0xFF, 0x07] # fullReverse = [0xFF, 0x03] leftBuff = createCommandPacket(leftWheel, 4, 0x20, multiplyVelocity(fullSpeedAhead, leftAxis)) rightBuff = createCommandPacket(rightWheel, 4, 0x20, multiplyVelocity(fullSpeedAhead, rightAxis)) buff = [] buff.append(leftBuff) buff.append(rightBuff) return buff
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) )
else: 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 ]------------------------
while True: [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 above ]------------------------ #----------------------------------------------------- # Main Loop # Def: # tim.sim[0] = Sim Time fullSpeedAhead = [0xFF, 0x07] leftWheel = 0x01 rightWheel = 0x00 print 'Sim Time = ', tim.sim[0] # Sleeps time.sleep(0.1) buff = createCommandPacket(leftWheel, 4, 0x20, multiplyVelocity(fullSpeedAhead, -0.5)) ref = ser.serial_sim(r,ref,buff) buff = createCommandPacket(rightWheel, 4, 0x20, multiplyVelocity(fullSpeedAhead, 0.5)) ref = ser.serial_sim(r,ref,buff) #----------------------------------------------------- #--------[ Do not edit below ]------------------------ #-----------------------------------------------------
print 'Sim Time = ', tim.sim[0] # Sleeps #time.sleep(0.1) i = 0 while i < 4: j = 0 while j < 4: #move straight fullSpeedAhead = [0xFF, 0x07] fullReverse = [0xFF, 0x03] fullStop = [0x00, 0x00] buff = createCommandPacket(0x00, 4, 0x20, fullSpeedAhead) ref = ser.serial_sim(r,ref,buff) buff = createCommandPacket(0x01, 4, 0x20, fullSpeedAhead) ref = ser.serial_sim(r,ref,buff) #wait for a couple seconds before slowing down down prev_tim = tim.sim[0] while tim.sim[0] < prev_tim + 4.0: time.sleep(0.05) [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 'Moving straight, Sim Time = ', tim.sim[0] else: raise ach.AchException( v.result_string(status) )
pass #print 'Sim Time = ', tim.sim[0] else: raise ach.AchException( v.result_string(status) ) #----------------------------------------------------- #--------[ Do not edit above ]------------------------ #----------------------------------------------------- # Main Loop # Def: # tim.sim[0] = Sim Time [statusp, framesizep] = p.get(PIDin, wait=False, last=True) fullSpeedAhead = [0xFF, 0x07] leftWheel = 0x01 rightWheel = 0x00 print 'Sim Time = ', tim.sim[0] # Sleeps time.sleep(0.1) #scale wheel speed by numbers passed in through PID channel buff = createCommandPacket(leftWheel, 4, 0x20, multiplyVelocity(fullSpeedAhead, PIDin.wheel[1])) ref = ser.serial_sim(r,ref,buff) buff = createCommandPacket(rightWheel, 4, 0x20, multiplyVelocity(fullSpeedAhead, PIDin.wheel[0])) ref = ser.serial_sim(r,ref,buff) print "wheel0: ", PIDin.wheel[0], " wheel1: ", PIDin.wheel[1] #----------------------------------------------------- #--------[ Do not edit below ]------------------------ #-----------------------------------------------------