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 ]------------------------
#-----------------------------------------------------