def goBackward(__self__, robot_num, dx, m: MQTTClient):
if robot_num != 1 and robot_num != 2:
print("goBackward called for invalid robot: " + str(robot_num))
return
if dx < 0 or dx > 244:
print("goBackward called for out of range dx: " + str(dx))
return
print("goBackward called for dx: " + str(dx))
time_count = 0
topic = None
if robot_num is 1:
time_count = str(round(1000 * (dx / __self__.robot1_avg_speed)))
topic = "esp32/m1t"
else:
time_count = str(round(1000 * (dx / __self__.robot2_avg_speed)))
topic = "esp32/m2t"
print("time_count: " + time_count)
if len(time_count) is 1:
time_count = '000' + time_count
elif len(time_count) is 2:
time_count = '00' + time_count
if len(time_count) is 3:
time_count = '0' + time_count
elif len(time_count) > 4:
print("Error with time_count!")
print("Final time_count: " + time_count)
m.publish(m, topic, "B" + time_count)
def goForwardSlowly(__self__, robot_num, m: MQTTClient):
print("goForwardSlowly called")
if robot_num != 1 and robot_num != 2:
print("Invalid robot_num passed: " + str(robot_num))
return
temp = __self__.slow_topic + str(robot_num)
m.publish(m, temp, "G")
def stopRobot(__self__, robot_num, m: MQTTClient):
if robot_num != 1 and robot_num != 2:
print("stopRobot called with invalid robot_num: " + str(robot_num))
return
print("stopRobot called for robot_num: " + str(robot_num))
topic = ""
if robot_num is 1:
m.publish(m, "esp32/r1", "S")
else:
m.publish(m, "esp32/r2", "S")
def turnRobot(__self__, robot_num, direction, m: MQTTClient):
if robot_num != 1 and robot_num != 2:
print("turnRobot called for invalid robot: " + str(robot_num))
return
if direction != 'L' and direction != 'R':
print("turnRobot called with invalid direction: " + str(direction))
print("turnRobot called for robot " + str(robot_num) + ", dir: " +
str(direction))
if robot_num == 1:
if direction == 'L':
m.publish(m, "esp32/m1t", __self__.robot1_timedleft90deg_msg)
elif direction == 'R':
m.publish(m, "esp32/m1t", __self__.robot1_timedright90deg_msg)
else:
if direction == 'L':
m.publish(m, "esp32/m2t", __self__.robot2_timedleft90deg_msg)
elif direction == 'R':
m.publish(m, "esp32/m2t", __self__.robot2_timedright90deg_msg)
# getDataJob is implemented by schedule to run at a certain time interval
# it updates the CV data in the SwarmData object
def getDataJob():
# print("Getting new CV data...")
S.getData(S)
S.robot2 = S.positions['blue']['circle']
S.robot1 = S.positions['blue']['square']
S.ball = S.positions['ball']
# print("R: " + str(S.robot[0]) + ", " + str(S.robot[1]))
# print("S: " + str(S.ball[0]) + ", " + str(S.ball[1]))
if __name__ == '__main__':
M.connectMQTT(M)
M.subscribe(M, topic="esp32/s")
getDataTimer = T(0.5, getDataJob) # schedules getDataJob
last_turn = None
next_turn = None
x_tolerance = 20
y_tolerance = 12
done = False
while done is False:
getDataJob()
print("Robot1 is at " + str(S.robot1[0]) + ", " + str(S.robot1[1]))
print("Robot2 is at " + str(S.robot2[0]) + ", " + str(S.robot2[1]))
print("Ball is at " + str(S.ball[0]) + ", " + str(S.ball[1]))
i = input("Type 0 to go, anything else to refresh")