def on_message(client, userdata, message):
message.payload = message.payload.decode("utf-8")
print("Received message " + str(message.payload) + " on topic " +
message.topic + ".")
msg = str(message.payload)
if msg == "stop":
pwm.stop()
elif msg == "forward":
str8.forward(50)
elif msg[0] == "_":
q = int(msg[1:5])
gyro.rotate(q)
elif message.topic == "blanotiger/robot3":
readIn(msg)
print("(" + str(botX) + ")(" + str(ballX) + ")")
findTheta()
findMid()
print("(" + str(midpointX) + ")")
global phase
phase = 1
driveF()
print("Rotating...")
rotateL(math.degrees(theta))
print("Finished Turning.")
phase = 2
driveF()
print("Destination Reached.")
else:
client.publish("blanotiger/debug",
payload="Error with Message.",
qos=0,
retain=False)
def on_message(client,userdata,message):
message.payload = message.payload.decode("utf-8")
print("Received message '"+str(message.payload)+"' on topic '"+message.topic+"'.")
msg = str(message.payload)
if msg=="forward":
msg=""
pwm.forward()
print("Forward.")
elif msg=="backward":
msg=""
pwm.backward()
print("Backward.")
elif msg=="stop":
msg=""
pwm.stop()
flag = 0
print("Stop.")
client.publish("blanotiger/movement", payload=gyro.angle, qos=0,retain=False)
elif msg=="forward left":
msg=""
pwm.forward_left()
print("Forward Left.")
elif msg=="forward right":
msg=""
pwm.forward_right()
print("Forward Right.")
elif msg=="spin":
msg=""
flag = 1
pwm.spin()
print("Spin")
def rotateL(value1):
client.publish("blanotiger/debug",
payload="Called rotateL(" + str(value1) + ").",
qos=0,
retain=False)
gyro.rotate(-value1)
pwm.stop()
def on_message(client, userdata, message):
message.payload = message.payload.decode("utf-8")
print("Received message '" + str(message.payload) + "' on topic '" +
message.topic + "'.")
msg = str(message.payload)
if msg == "stop":
pwm.stop()
elif msg == "forward":
str8.forward(50)
elif msg[0] == "_":
q = msg[1:5]
print("Turning " + str(q) + " degrees.")
gyro.rotate(q)
elif message.topic == "blanotiger/robot1":
readIn(msg)
print("(" + str(botX) + ", " + str(botY) + ")(" + str(ballX) + ", " +
str(ballY) + ")")
findMid()
print("(" + str(midpointX) + ", " + str(midpointY) + ")")
global phase
phase = 1
driveF()
print("Turning...")
rotateR(-30)
print("Finished Turning.")
phase = 2
driveF()
print("Destination Reached.")
def driveF():
global phase
global midpointX
global botX
global botY
global ballY
global ballX
newparsed = ' '
if phase == 1:
while botX > midpointX:
str8.forward(10)
pwm.stop()
time.sleep(0.5)
oldparsed = newparsed
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Circle"]["Object Center"][
"X"]
while oldparsed == newparsed:
print("Data did not update.")
time.sleep(0.5)
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed
botX = float(
parsed["Blue Team Data"]["Circle"]["Object Center"]["X"])
client.publish("blanotiger/debug",
payload="X: " + str(botX),
qos=0,
retain=False)
print("botX: " + str(botX))
elif phase == 2:
while (botX > ballX) or (botY > ballY):
str8.forward(10)
pwm.stop()
time.sleep(0.5)
oldparsed = newparsed
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Circle"]["Object Center"][
"X"]
while oldparsed == newparsed:
print("Data did not update.")
time.sleep(0.5)
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Circle"][
"Object Center"]["X"]
botX = float(
parsed["Blue Team Data"]["Circle"]["Object Center"]["X"])
botY = float(
parsed["Blue Team Data"]["Circle"]["Object Center"]["Y"])
client.publish("blanotiger/debug",
payload="Bot: (" + str(botX) + "," + str(botY) +
")",
qos=0,
retain=False)
print("Done.")
def rotate(value1):
angle = 0
value1 = value1 * 7 / 10
b = SMBus(1)
L3G = 0x6b
who = 0b11010111
Z_MSB = 0x2d
Z_LSB = 0x2c
CTRL_GYRO_1 = 0x20
CTRL_GYRO_2 = 0x21
CTRL_GYRO_6 = 0x39
b.write_byte_data(L3G, CTRL_GYRO_1, 0b00001111)
b.write_byte_data(L3G, CTRL_GYRO_2, 0x00)
b.write_byte_data(L3G, CTRL_GYRO_6, 0b000000)
sens = .00875
value2 = True
while value2:
start = time.time()
z = twos_comp_combine(b.read_byte_data(L3G, Z_MSB),
b.read_byte_data(L3G, Z_LSB))
zdps = z * sens
heading = zdps * time_div(start)
angle += heading
print(angle)
if value1 > 0:
if (abs(angle) >= 360):
angle = 0
if angle < value1:
pwm.lspin()
else:
pwm.stop()
value2 = False
elif value1 < 0:
if (abs(angle) >= 360):
angle = 0
if angle > value1:
pwm.rspin()
else:
pwm.stop()
value2 = False
def rotate(value1):
angle = 0
value2 = True
turnVal = "right"
global start
start = time.time()
while value2:
angle += sample()
print(angle)
if (abs(angle) >= 360):
angle = 0
if value1 > 0:
if angle < value1:
angle += sample()
pwm.lspin()
turnVal = "left"
angle += sample()
else:
pwm.stop()
value2 = False
angle += sample()
elif value1 < 0:
if angle > value1:
angle += sample()
pwm.rspin()
turnVal = "right"
angle += sample()
else:
pwm.stop()
value2 = False
angle += sample()
angle += sample()
while not ((abs(angle) > abs(value1) - .5) &
(abs(angle) < abs(value1) + .5)):
angle += sample()
print(angle)
if (turnVal == "right"):
angle += sample()
pwm.lspin()
angle += sample()
else:
angle += sample()
pwm.rspin()
angle += sample()
pwm.stop()
#import pdb
import encoder as str8
import rotateFunction as rotate
import pwm0 as pwm
import time
import sys
while True:
str8.forward(10)
pwm.stop()
time.sleep(1)
rotate.rotate(180)
pwm.stop()
time.sleep(1)
str8.forward(10)
pwm.stop()
time.sleep(1)
rotate.rotate(-180)
pwm.stop()
time.sleep(1)
def driveF():
global phase
client.publish("blanotiger/debug",
payload="driveF(" + str(phase) + ") called.",
qos=0,
retain=False)
global midpointX
global botX
global botY
global ballY
global ballX
newparsed = ' '
if phase == 1:
while botX > midpointX:
str8.forward(10)
pwm.stop()
time.sleep(0.5)
oldparsed = newparsed
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Triangle"]["Object Center"][
"X"]
while oldparsed == newparsed:
print("Data did not update.")
client.publish("blanotiger/debug",
payload="Data didn't update.",
qos=0,
retain=False)
time.sleep(0.5)
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Triangle"][
"Object Center"]["X"]
botX = float(
parsed["Blue Team Data"]["Triangle"]["Object Center"]["X"])
client.publish("blanotiger/debug",
payload="X: " + str(botX),
qos=0,
retain=False)
print("botX: " + str(botX))
print("Stopping...")
client.publish("blanotiger/debug",
payload="Movement 1 Complete.",
qos=0,
retain=False)
elif phase == 2:
str8.forward(10)
while (botX >= ballX) or (botY >= ballY):
time.sleep(0.5)
oldparsed = newparsed
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Triangle"]["Object Center"][
"X"]
while oldparsed == newparsed:
print("Data did not update.")
client.publish("blanotiger/debug",
payload="Data didn't update.",
qos=0,
retain=False)
time.sleep(0.5)
r = requests.get(getPath, timeout=2)
parsed = json.loads(r.text)
newparsed = parsed["Blue Team Data"]["Triangle"][
"Object Center"]["X"]
botX = float(
parsed["Blue Team Data"]["Triangle"]["Object Center"]["X"])
botY = float(
parsed["Blue Team Data"]["Triangle"]["Object Center"]["Y"])
client.publish("blanotiger/debug",
payload="Bot: (" + str(botX) + "," + str(botY) +
")",
qos=0,
retain=False)
client.publish("blanotiger/fin",
payload="Bot 3 Complete",
qos=0,
retain=False)
else:
client.publish("blanotiger/debug",
payload="Error: " + str(value1),
qos=0,
retain=False)
def rotateR(value1):
gyro.rotate(value1)
pwm.stop()
