def Initialize_zeros():
print("Zeroing launcher motor speed...")
zero_speed()
print("Zeroing aiming...")
zero_angleX()
zero_angleY()
anglefileY = open("current_angleY.txt")
prevangleY = float(anglefileY.readline().strip())
anglefileY.close()
# print prevangleX
# print prevangleY
if anglesX[0] != prevangleX:
setangleX(15, prevangleX)
prevangleX = 15
if anglesY[0] != prevangleY:
setangleY(0, prevangleY)
prevangleY = 0
print("Zeroing launcher motor speed...")
zero_speed()
print("Powering on launcher motors...")
MainMotorPowerON()
time.sleep(5) #Motors need spinup time
for puck in IDs:
if(puck == 0):
print "Puck", puck
#Set motor speeds
print "Setting puck", puck, "motor speed..."
setspeed(wheelspeeds[puck], prevspeed)
prevspeed = wheelspeeds[puck]
#Set angles
print "Setting puck", puck, "angle (X)..."