def __init__(self):
super(robot_gui, self).__init__()
self.robot = Robot()
self.robot.initialise()
self.microsteps = 2
self.rpm = 100
self.intervals = 500
self.direction = 1
self.max_runs = 10
self.running = False
self.position = 0
self.jog_mode = False
self.probe_data = []
self.errors = 0
self.obj = Worker() # no parent!
self.obj.finished.connect(self.status_bar_done)
self.obj.position.connect(self.update_lcd)
self.ui = Ui_MainWindow()
self.setupUi(self)
self.link_buttons()
self.statusBar().showMessage("Ready")
def control_humanoid():
robot = Robot.get_inst(1)
object_detector = ObjectDetector()
try:
# Loop until exit flag set to True(1)
while not ExitControl.gen:
# Get users desired action
motion_num = input("Enter motion num: ")
# Give access to full range of motions
if motion_num == KeyWordControl.FULL_CONTROL:
print("full control")
motion_num = input("Enter motion num: ")
# Print dictionary
if motion_num == KeyWordControl.DICTIONARY:
for key in DICTS.HUMANOID_FULL:
print("{} :{} ".format(key, DICTS.HUMANOID_FULL[key]))
motion_num = input("Enter motion num: ")
try:
motion_num = int(motion_num)
except ValueError as e:
print("{} is not a number, full control"
" exit".format(motion_num))
print(e)
if motion_num in DICTS.HUMANOID_FULL:
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[motion_num])
# Create keyboard remote
elif motion_num == KeyWordControl.REMOTE:
alert_window = AlertWindow("remote")
alert_window.output("Enter a key")
ExitControl.remote = 0
# Loop until exit flag set to 1
while not ExitControl.remote:
# get users command
command = cv2.waitKey(0)
# Stop robot motion
if command == R_Control.STOP:
message = "Robot stop"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[1])
# Enable and disable detection motion command
elif command == R_Control.DETECT_ON:
message = "detect on"
alert_window.output(message)
Funcs.detect_on()
elif command == R_Control.DETECT_OFF:
message = "detect off"
alert_window.output(message)
Funcs.detect_off()
# Move the robot
elif command == R_Control.FORWARD:
message = "forward"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[15])
elif command == R_Control.BACKWARD:
message = "backward"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[16])
elif command == R_Control.LEFT:
message = "left"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[17])
elif command == R_Control.RIGHT:
message = "right"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[18])
# Turn the robot
elif command == R_Control.TURN:
message = "turn"
alert_window.output(message)
command = cv2.waitKey(0)
if command == R_Control.LEFT:
message = "turn left"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[19])
elif command == R_Control.RIGHT:
message = "turn right"
alert_window.output(message)
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[20])
else:
message = "invalid turn"
alert_window.output(message)
# Exit program
elif command == R_Control.EXIT:
ExitControl.gen = 1
ExitControl.remote = 1
print("exiting")
# Close remote
elif command == R_Control.CLOSE:
ExitControl.remote = 1
else:
message = "invalid option"
alert_window.output(message)
# Close window object (close opencv window)
del alert_window
# Print dictionary
elif motion_num == KeyWordControl.DICTIONARY:
for key in DICTS.HUMANOID_MOTION:
print(key, " : ", DICTS.HUMANOID_MOTION[key])
# Stop robot current motion
elif (motion_num == KeyWordControl.STOP1
or motion_num == KeyWordControl.STOP2):
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[1])
# Start calibration
elif motion_num == KeyWordControl.CALIBRATE:
# Ensure calibrator thread is not already active
if ("calibrator_thread" not in locals()
or not calibrator_thread.is_alive()):
print("starting calibrate")
calibrator_thread = threading.Thread(
target=object_detector.calibrate_filter)
with Locks.CAM_LOCK:
ExitControl.calibrate = 0
calibrator_thread.start()
else:
print("calibrate already active")
# Stop Calibration
elif motion_num == KeyWordControl.CALIBRATE_STOP:
print("stopping calibrate")
with Locks.CAM_LOCK:
ExitControl.calibrate = 1
# Enable or disable ball detection from sending motion command
elif motion_num == KeyWordControl.DETECT_ON:
Funcs.detect_on()
elif motion_num == KeyWordControl.DETECT_OFF:
Funcs.detect_off()
# Turn debug verbose on or off
elif motion_num == KeyWordControl.DEBUG_ON:
Debug.debug_on()
elif motion_num == KeyWordControl.DEBUG_OFF:
Debug.debug_off()
# Create exit condition
elif (motion_num == KeyWordControl.EXIT1
or motion_num == KeyWordControl.EXIT2):
Funcs.exit_program()
else:
try:
motion_num = int(motion_num)
except ValueError:
print("motion unknown try again")
if motion_num in DICTS.HUMANOID_MOTION:
robot.play_rcb4_motion(HexConst.HUNDRED_NUM[motion_num])
except Exception as e:
print("Main program exiting with exception: \n", e)
finally:
with Locks.GEN_LOCK:
# Set exit flag for all threads
ExitControl.gen = 1
del robot
del object_detector
import RPi.GPIO as GPIO
import time
from robot_control import Robot
GPIO.cleanup()
GPIO.setmode(GPIO.BOARD)
robot = Robot()
robot.SetSpeed(90)
turn_distance = 20
while True:
distance = robot.GetUltraSonicDistance()
print(distance)
if(distance < turn_distance):
robot.SpinClockwise()
time.sleep(1)
else:
robot.MoveForward()
class robot_gui(Ui_MainWindow):
def __init__(self):
super(robot_gui, self).__init__()
self.robot = Robot()
self.robot.initialise()
self.microsteps = 2
self.rpm = 100
self.intervals = 500
self.direction = 1
self.max_runs = 10
self.running = False
self.position = 0
self.jog_mode = False
self.probe_data = []
self.errors = 0
self.obj = Worker() # no parent!
self.obj.finished.connect(self.status_bar_done)
self.obj.position.connect(self.update_lcd)
self.ui = Ui_MainWindow()
self.setupUi(self)
self.link_buttons()
self.statusBar().showMessage("Ready")
def update_lcd(self, i):
if 0 <= i < 10:
self.motor_pos_lcd.setDigitCount(4)
elif -10 < i < 0 or i >= 10:
self.motor_pos_lcd.setDigitCount(5)
elif i <= -10 or i >= 100:
self.motor_pos_lcd.setDigitCount(6)
elif i <= -100 or i >= 1000:
self.motor_pos_lcd.setDigitCount(7)
self.motor_pos_lcd.display(i)
def status_bar_done(self):
self.statusBar().showMessage("Done")
self.start_run_button.setDisabled(False)
self.stop_run_button.setDisabled(True)
def showDialog(self):
fname = QFileDialog.getSaveFileName(self, 'Save File', 'data.csv')
with open(fname[0], "w", newline='') as csvfile:
csvwriter = csv.writer(csvfile)
csvwriter.writerow(("Position", "Encoder", "A0", "A1", "A2"))
for row in self.probe_data:
csvwriter.writerow(row)
def link_buttons(self):
self.link_speed_buttons()
self.link_microstep_buttons()
self.link_jog_buttons()
self.stop_run_button.clicked.connect(self.stop)
self.start_run_button.clicked.connect(self.run)
self.pushButton.clicked.connect(self.zero)
self.pushButton_2.clicked.connect(self.showDialog)
def link_jog_buttons(self):
self.jog_up_fast.clicked.connect(lambda: self.set_jog_speed("1500"))
self.jog_up_slow.clicked.connect(lambda: self.set_jog_speed("100"))
self.jog_stop.clicked.connect(self.set_jog_stop)
self.jog_down_slow.clicked.connect(lambda: self.set_jog_speed("-100"))
self.jog_down_fast.clicked.connect(lambda: self.set_jog_speed("-1500"))
@pyqtSlot()
def set_jog_speed(self, speed):
self.jog_mode = False
self.robot.jog_speed(speed)
self.jog_mode = True
threading.Thread(target=self.run_jog_mode).start()
@pyqtSlot()
def set_jog_stop(self):
self.robot.jog_speed(0)
sleep(0.1)
self.robot.jog_speed(0)
self.jog_stop.setAutoExclusive(False)
self.jog_stop.setChecked(False)
self.jog_stop.setAutoExclusive(True)
self.jog_mode = False
def stop(self):
self.running = False
def run(self):
self.running = True
self.start_run_button.setDisabled(True)
self.stop_run_button.setDisabled(False)
self.statusBar().showMessage("Running")
threading.Thread(target=self.run_commands).start()
def zero(self):
# Update motor position QLCDNumber widget
self.obj.position.emit(self.robot.zero())
def link_speed_buttons(self):
self.speed_50.pressed.connect(lambda: self.set_speed(self.speed_50))
self.speed_100.pressed.connect(lambda: self.set_speed(self.speed_100))
self.speed_150.pressed.connect(lambda: self.set_speed(self.speed_150))
self.speed_200.pressed.connect(lambda: self.set_speed(self.speed_200))
def link_microstep_buttons(self):
self.microstep_1.pressed.connect(
lambda: self.set_microsteps(self.microstep_1))
self.microstep_2.pressed.connect(
lambda: self.set_microsteps(self.microstep_2))
self.microstep_4.pressed.connect(
lambda: self.set_microsteps(self.microstep_4))
self.microstep_8.pressed.connect(
lambda: self.set_microsteps(self.microstep_8))
def set_speed(self, b):
self.rpm = int(b.text()[:3])
def set_microsteps(self, b):
self.microsteps = int(b.text()[0])
def handle_sensor_data(self, line):
data = self.robot.handle_sensor_data(line)
# Update motor position QLCDNumber widget
self.obj.position.emit(self.robot.position)
# Check data isn't blank
if len(data) == 5:
# Add readings to dataset
self.probe_data.append(data)
else:
self.errors += 1
def handle_status_message(self, line):
self.position = self.robot.handle_status_message(line)
# Update motor position QLCDNumber widget
self.obj.position.emit(self.position)
# Check data isn't blank
def run_jog_mode(self):
# Run until jog mode flag is set to False
while self.jog_mode:
# Try/Except statements as serial read can fail
try:
# Check for available data to read in
if self.robot.xbee.inWaiting():
# Decode and strip EOL characters
line = self.robot.get_cleaned_line()
# Indicates a robot status string
if line[0] == "s":
self.handle_status_message(line)
# Any other string is invalid
else:
print("Invalid String: {0}".format(line))
# Debug print any failure in the above code
except Exception as e:
self.errors += 1
print("Exception: {0}".format(e))
self.handle_end_data()
def run_commands(self):
start_time = time()
# Generate a command string from set variables
self.robot.construct_command(
MIC=self.microsteps,
STP=self.rpm,
INT=self.intervals,
DIR=self.direction,
RUN=1)
# Start first run
self.robot.send_command()
# Variable to keep track of how many runs have happened
current_run = 0
# Keep track if end stop hit
end_hit = False
# Run until self.running flag is set to False
while self.running:
# Try/Except statements as serial read can fail
try:
# Check for available data to read in
if self.robot.xbee.inWaiting():
# Decode and strip EOL characters
line = self.robot.get_cleaned_line()
# Get first character in string to indicate type
indicator = line[0]
# Indicates a sensor data string
if indicator == "v":
self.handle_sensor_data(line)
# Indicates a robot status string
elif indicator == "s":
self.handle_status_message(line)
# Indicates completion of last command set
elif line == "ack":
# Move to next run through commands
# current_run += 1
# Stop sending command strings if at max runs
if current_run == self.max_runs or end_hit:
self.running = False
# Send repeat command to keep running
else:
self.robot.send_command()
elif line == "END":
end_hit = True
# Debug output for invalid data string
else:
self.errors += 1
print("Invalid String: {0}".format(line))
# Debug print any failure in the above code
except Exception as e:
self.errors += 1
print("Exception: {0}".format(e))
# Wait for any remaining serial data
self.handle_end_data()
# Update status bar to "Done" and enable/disable run/stop buttons
self.obj.finished.emit()
print("Error rate: {0}%".format(
self.errors / len(self.probe_data) * 100))
print("Frequncy: {0}Hz".format(
len(self.probe_data) / (time() - start_time)))
def handle_end_data(self):
# Wait for any remaining serial data
sleep(0.1)
# Handle last few status messages
while self.robot.xbee.inWaiting():
try:
# Decode and strip EOL characters
line = self.robot.get_cleaned_line()
# Indicates a robot status string
if line[0] == "v":
self.handle_sensor_data(line)
# Indicates a robot status string
elif line[0] == "s":
self.handle_status_message(line)
except:
self.errors += 1
print(self.errors)
pass
sleep(0.005)
def control_robot(self):
# All relevant command numbers can be found in config.Conf
# HUMANOID_FULL and SPIDER_FULL
if not self.is_connected:
self.logger.debug(
"Robot control could not be started as camera is not "
"connected")
return
from robot_control import Robot
self.logger.debug("control_robot: Started")
self.robot = Robot.get_inst(self.robot_type)
rbt_hd_srch_thrd = threading.Thread(target=self.rbt_head_search)
rbt_hd_track_thrd = threading.Thread(target=self.rbt_head_track)
cmd_sent = None
cmd_sent_time = 0
wait_time = 0
time.sleep(3)
while ExitControl.gen and ExitControl.cam:
success = False
dur = time.time() - cmd_sent_time
orig_wait_time = wait_time
if self.cam_obj_dict[ObjDist.IS_FOUND]:
if not rbt_hd_track_thrd.is_alive():
rbt_hd_track_thrd = threading.Thread(
target=self.rbt_head_track)
rbt_hd_track_thrd.start()
self.search_turn = False
self.last_non_search = time.time()
if dur > wait_time:
if ( # Within kick range
Conf.KICK_DIST + Conf.KICK_RANGE >
self.cam_obj_dict[ObjDist.AVG] >
Conf.KICK_DIST - Conf.KICK_RANGE):
cmd_sent = Conf.CMD_KICK
elif self.cam_obj_dict[ObjDist.AVG] > Conf.KICK_DIST:
cmd_sent = Conf.CMD_FORWARD
else:
cmd_sent = Conf.CMD_BACKWARD
self.command = self.robot.get_command_num(cmd_sent)
wait_time = self.robot.full_dict[self.command][1]
cmd_sent_time = time.time()
success = self.robot.send_command(self.command, auto=True)
else: # Search for object
non_search_dur = time.time() - self.last_non_search
if non_search_dur < Conf.MAX_SEARCH_DUR:
# Search in front of robot then turn robot around 180
if not rbt_hd_srch_thrd.is_alive():
rbt_hd_srch_thrd = threading.Thread(
target=self.rbt_head_search)
if self.search_turn:
cmd_sent = Conf.CMD_RIGHT
self.command = self.robot.get_command_num(cmd_sent)
wait_time = self.robot.full_dict[self.command][1]
self.robot.send_command(self.command, auto=True)
temp_dur = 0
wait_start = time.time()
temp_wait = wait_time / 10
while (temp_dur < wait_time
and not self.cam_obj_dict[ObjDist.IS_FOUND]
and self.robot.active_auto_control):
time.sleep(temp_wait)
temp_dur = time.time() - wait_start
self.search_turn = False
rbt_hd_srch_thrd.start()
else:
self.logger.warning(
"Object not found in "
f"{pretty_time(non_search_dur, is_raw=False)}"
" and robot has stopped searching",
log_type=Conf.LOG_CAM_STOP_SEARCH)
if success and dur > orig_wait_time:
self.logger.info(f"control_robot: Command sent: {cmd_sent}")
if self.disp:
if (self.action_request == Conf.CMD_RH_UP
or self.action_request == Conf.CMD_RH_DOWN
or self.action_request == Conf.CMD_RH_LEFT
or self.action_request == Conf.CMD_RH_RIGHT):
self.robot.send_head_command(self.action_request)
elif self.action_request == Conf.CMD_CV_HEAD_DELTA_P:
self.robot.head_delta_theta += 5
self.robot.logger.debug(
"Camera: control_robot: head_delta_theta increased to"
f" {self.robot.head_delta_theta} ")
elif self.action_request == Conf.CMD_CV_HEAD_DELTA_M:
self.robot.head_delta_theta -= 5
self.robot.logger.debug(
"Camera: control_robot: head_delta_theta decreased to"
f" {self.robot.head_delta_theta} ")
elif self.action_request == Conf.CMD_VARS:
self.robot.dump_status()
elif self.action_request == Conf.CMD_VARS1:
self.robot.dump_conf()
elif self.action_request == Conf.CMD_VARS2:
self.dump_status()
elif self.action_request != "": # Send all viable commands
if self.action_request == Conf.CMD_STOP:
self.robot.active_auto_control = False
self.robot.send_command(self.action_request)
if self.robot.cam_request == Conf.CMD_VARS2:
self.dump_status()
elif self.action_request != "":
temp = (
"control_robot: current action_request before erasing "
f"--> {self.action_request}")
print(temp)
self.action_request = ""
if self.robot.cam_request != "":
self.robot.cam_request = ""
time.sleep(.2)
else:
time.sleep(1)
