m4jez92wv
/
PyGame
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

修改

Browse Source
master
abc15379 2 years ago
parent 5062383ba4
commit 075fef94f9
8 changed files with 468 additions and 1285 deletions
Show Stats Download Patch File Download Diff File

BIN
src/windows/control/controltools/__pycache__/regPictrue.cpython-311.pyc
View File

Binary file not shown.

962
src/windows/control/controltools/contest(1).py
Unescape View File

@ -1,962 +0,0 @@
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
import tf_transformations
import threading
import numpy as np
import os
from configparser import ConfigParser
import pygame
class Drive(object):
def __init__(self) -> None:
self.valueinit()
self.ros_register()
def valueinit(self):
if os.path.exists('config.ini'):
config = ConfigParser()
with open('config.ini') as f:
config.read_file(f)
self.threshold_l = float(config.get('threshold', 'threshold_l'))
self.threshold_lf = float(config.get('threshold', 'threshold_lf'))
self.threshold_f = float(config.get('threshold', 'threshold_f'))
self.threshold_rf = float(config.get('threshold', 'threshold_rf'))
self.threshold_r = float(config.get('threshold', 'threshold_r'))
self.fyaw = float(config.get('threshold', 'fyaw'))
self.ryaw = float(config.get('threshold', 'ryaw'))
self.byaw = float(config.get('threshold', 'byaw'))
self.lyaw0 = float(config.get('threshold', 'lyaw0'))
self.lyaw1 = float(config.get('threshold', 'lyaw1'))
self.blocksize = float(config.get('threshold', 'blocksize'))
self.kp = float(config.get('threshold', 'kp'))
self.speed_x = float(config.get('threshold', 'speed_x'))
self.speed_z = float(config.get('threshold', 'speed_z'))
else:
self.threshold_l = 0.085
self.threshold_lf = 0.12
self.threshold_f = 0.078
self.threshold_rf = 0.12
self.threshold_r = 0.085
self.fyaw = 4.71
self.ryaw = 3.14
self.byaw = 1.57
self.lyaw0 = 0
self.lyaw1 = 6.28
self.blocksize = 0.17
self.kp = 10
self.speed_x = 0.10
self.speed_z = 0.35
self.yaw = 0
self.l_dis = 0
self.fl_dis = 0
self.f_dis = 0
self.fr_dis = 0
self.r_dis = 0
self.position_x = 0
self.position_y = 0
def ros_register(self):
self.msg = Twist()
self.string = String()
self.node = Node('mynode')
self.node.create_subscription(LaserScan, '/scan', self.laser, 10)
self.node.create_subscription(Odometry, '/odom', self.odom, 10)
self.pub = self.node.create_publisher(Twist, '/cmd_vel', 10)
self.pub_custom = self.node.create_publisher(String, '/mycustom', 10)
# self.timer = self.node.create_timer(0.05, self.ros_pub)
self.rate = self.node.create_rate(50.0)
def ros_pub(self):
self.msg.linear.x = self.speed_x
self.msg.angular.z = self.speed_z
self.pub.publish(self.msg)
def ros_spin(self):
rclpy.spin(self.node)
def laser(self, msg):
region = msg.ranges
self.l_dis = region[340]
self.fl_dis = region[270]
self.f_dis = region[180]
self.fr_dis = region[90]
self.r_dis = region[20]
def get_laser(self):
return self.l_dis, self.fl_dis, self.f_dis, self.fr_dis, self.r_dis
def odom(self, msg):
position = msg.pose.pose.position
self.position_x = position.x
self.position_y = position.y
quaternion = (
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
euler = tf_transformations.euler_from_quaternion(quaternion)
self.yaw = euler[2] + 3.1415926
def get_odom(self):
return self.position_x, self.position_y, self.yaw
def posture_adjust(self):
if self.f_dis > 0.09:
if self.fl_dis < 0.118:
speed_z_temp = 0.5 * (self.fl_dis - 0.118) * self.kp
elif self.fr_dis < 0.118:
speed_z_temp = -0.5 * (self.fr_dis - 0.118) * self.kp
else:
speed_z_temp = 0.0
else:
if self.l_dis < 0.085:
speed_z_temp = 0.5 * (self.l_dis - 0.085) * self.kp
elif self.r_dis < 0.085:
speed_z_temp = -0.5 * (self.r_dis - 0.085) * self.kp
else:
speed_z_temp = 0.0
return speed_z_temp
def move(self, numblock=1):
flag = 1
flag_cps = 0
compensate = 0
while rclpy.ok():
self.msg.linear.x = self.speed_x
self.msg.angular.z = self.posture_adjust()
self.pub.publish(self.msg)
self.rate.sleep()
if flag:
tempx, tempy, tempz = self.get_odom()
if (not tempx) and (not tempy):
continue
flag = 0
if self.l_dis < 0.1 and self.r_dis < 0.1:
flag_cps = 0
elif self.l_dis < 0.1 and self.r_dis > 0.1:
flag_cps = 1
if self.l_dis > 0.1 and self.r_dis < 0.1:
flag_cps = 2
elif self.l_dis > 0.1 and self.r_dis > 0.1:
flag_cps = 3
if 0.01 < self.f_dis < 0.075:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
if abs(self.get_odom()[0] - tempx) >= self.blocksize * numblock or abs(
self.get_odom()[1] - tempy) >= self.blocksize * numblock:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
if flag_cps == 0 and (self.l_dis > 0.1 or self.r_dis > 0.1):
flag = 1
compensate = 1
break
"""
# 其他情况，暂时没想好怎么处理，所以注释掉
if flag_cps ==1 and self.l_dis>0.1:
flag = 1
compensate = 0
break
if flag_cps ==2 and self.r_dis>0.1:
flag = 1
compensate = 0
break
"""
while compensate and rclpy.ok():
self.msg.linear.x = self.speed_x
self.msg.angular.z = self.posture_adjust()
self.pub.publish(self.msg)
self.rate.sleep()
if flag:
tempx, tempy, tempz = self.get_odom()
if (not tempx) and (not tempy):
continue
flag = 0
if 0.01 < self.f_dis < 0.075:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
if abs(self.get_odom()[0] - tempx) >= self.blocksize / 2 + 0.01 or abs(
self.get_odom()[1] - tempy) >= self.blocksize / 2 + 0.01:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
def turn(self, angle):
while rclpy.ok():
self.msg.linear.x = 0.0
self.msg.angular.z = -self.speed_z
self.pub.publish(self.msg)
self.rate.sleep()
if angle - 0.1 < self.yaw < angle + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
def turnright(self):
flag = 1
while rclpy.ok():
self.msg.linear.x = 0.0
self.msg.angular.z = -self.speed_z
self.pub.publish(self.msg)
self.rate.sleep()
if flag: # It is similar with move.
oldyaw = self.get_odom()[2]
if not oldyaw:
continue
flag = 0
# Considering the mutation of 0 and 6.28, so dividing the whole process into four parts.
if 4.41 < oldyaw < 5.01: # up
if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
elif 2.84 < oldyaw < 3.44: # right
if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
elif 1.27 < oldyaw < 1.87: # back
if self.yaw < self.lyaw0 + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
elif oldyaw < 0.3 or oldyaw > 5.98: # left
if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
def turnleft(self):
flag = 1
while rclpy.ok():
self.msg.linear.x = 0.0
self.msg.angular.z = self.speed_z
self.pub.publish(self.msg)
self.rate.sleep()
if flag:
oldyaw = self.yaw
if not oldyaw:
continue
flag = 0
if 4.41 < oldyaw < 5.01: # up
if self.yaw > self.lyaw1 - 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
print(1111)
break
elif 2.84 < oldyaw < 3.44: # right
if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
print(222)
break
elif 1.27 < oldyaw < 1.87: # back
if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
print(44444)
break
elif oldyaw < 0.3 or oldyaw > 5.98: # left
if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
print(155555)
break
def turnback(self):
flag = 1
while rclpy.ok():
self.msg.linear.x = 0.0
self.msg.angular.z = -self.speed_z
self.pub.publish(self.msg)
self.rate.sleep()
if flag:
oldyaw = self.get_odom()[2]
if not oldyaw:
continue
flag = 0
if 4.41 < oldyaw < 5.01: # up
if self.byaw - 0.1 < self.yaw < self.byaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
elif 2.84 < oldyaw < 3.44: # right
if self.yaw < self.lyaw0 + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
elif 1.27 < oldyaw < 1.87: # back
if self.fyaw - 0.1 < self.yaw < self.fyaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
elif oldyaw < 0.3 or oldyaw > 5.98: # left
if self.ryaw - 0.1 < self.yaw < self.ryaw + 0.1:
self.msg.linear.x = 0.0
self.msg.angular.z = 0.0
self.pub.publish(self.msg)
# rclpy.spin_once(self.node)
self.rate.sleep()
break
class Micromouse(Drive):
def __init__(self):
"""继承父类，禁止删减，但可以增加自己要添加的内容。"""
super().__init__()
self.valuesinit()
def valuesinit(self):
"""Micromosue参数初始化, 禁止修改"""
self.mapblock = np.full((16, 16), '00000') # 2D array,墙壁资料初始化, flag left down right up
# flag: 是否走过, 0 or 1
# left: 是否有路, 0 or 1
# down: 是否有路, 0 or 1
# right: 是否有路, 0 or 1
# up: 是否有路, 0 or 1
self.mapblock[0, 0] = '10001' # 起点墙壁初始化
self.crosswaystack = []
self.mouse_x = 0 # x,y起点坐标，这是在迷宫中的坐标
self.mouse_y = 0
self.dir = 0 # Micromouse车头,0:up, 1:right, 2:down, 3:left
def __coordinateupdate(self):
"""根据车头方向更新坐标, 禁止修改"""
if self.dir == 0: # 车头向上
self.mouse_y += 1
elif self.dir == 1: # 车头向右
self.mouse_x += 1
elif self.dir == 2: # 车头向下
self.mouse_y -= 1
elif self.dir == 3: # 车头向左
self.mouse_x -= 1
def get_coordinate(self):
"""获取坐标，方便调用，禁止修改"""
return self.mouse_x, self.mouse_y
def __savewallinfo(self):
"""墙壁信息保存, 禁止修改"""
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
# 墙壁资料赋值
if self.dir == 0:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', leftpath, backpath, rightpath, frontpath))
elif self.dir == 1:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', backpath, rightpath, frontpath, leftpath))
elif self.dir == 2:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', rightpath, frontpath, leftpath, backpath))
elif self.dir == 3:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', frontpath, leftpath, backpath, rightpath))
def turnright(self):
"""右转弯，加入了方向转换, 禁止修改"""
self.dir = (self.dir + 1) % 4
return super(Micromouse, self).turnright()
def turnleft(self):
"""左转弯，加入了方向转换, 禁止修改"""
self.dir = (self.dir + 3) % 4
return super(Micromouse, self).turnleft()
def turnback(self):
"""向后转弯，加入了方向转换, 禁止修改"""
self.dir = (self.dir + 2) % 4
return super(Micromouse, self).turnback()
def moveoneblock(self):
"""移动一个单元格，然后更新坐标，保存墙壁信息的功能, 禁止修改"""
self.move()
self.__coordinateupdate()
if self.mapblock[self.mouse_x, self.mouse_y][0] == '0':
self.__savewallinfo()
self.ros_pub_custom()
def ros_pub_custom(self):
data = ''.join((str(self.mouse_x), ',',
str(self.mouse_y), ',',
str(self.dir), ',',
self.mapblock[self.mouse_x, self.mouse_y][1:]
))
self.string.data = data
self.pub_custom.publish(self.string)
def rightmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
def leftmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
def frontrightmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
def frontleftmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
def centralmethod(self):
if self.mouse_x < 8:
if self.mouse_y < 8: # Micromouse位于左下角
if self.dir == 0:
self.frontrightmethod()
elif self.dir == 1:
self.frontleftmethod()
elif self.dir == 2:
self.leftmethod()
elif self.dir == 3:
self.rightmethod()
else: # Micromouse位于左上角
if self.dir == 0:
self.rightmethod()
elif self.dir == 1:
self.frontrightmethod()
elif self.dir == 2:
self.frontleftmethod()
elif self.dir == 3:
self.leftmethod()
else:
if self.mouse_y < 8: # Micromouse位于右下角
if self.dir == 0:
self.frontleftmethod()
elif self.dir == 1:
self.leftmethod()
elif self.dir == 2:
self.rightmethod()
elif self.dir == 3:
self.frontrightmethod()
else: # Micromouse位于右下角
if self.dir == 0:
self.leftmethod()
elif self.dir == 1:
self.rightmethod()
elif self.dir == 2:
self.frontrightmethod()
elif self.dir == 3:
self.frontleftmethod()
def centralmethod2(self):
if self.mouse_x in (7, 8) or self.mouse_y in (7, 8):
if self.mouse_y in (7, 8):
if self.mouse_x < 8:
if self.dir == 0:
self.rightmethod()
elif self.dir == 1:
self.frontrightmethod()
elif self.dir == 2:
self.leftmethod()
elif self.dir == 3:
self.frontrightmethod()
else:
if self.dir == 0:
self.leftmethod()
elif self.dir == 1:
self.frontrightmethod()
elif self.dir == 2:
self.rightmethod()
elif self.dir == 3:
self.frontrightmethod()
if self.mouse_x in (7, 8):
if self.mouse_y < 8:
if self.dir == 0:
self.frontrightmethod()
elif self.dir == 1:
self.leftmethod()
elif self.dir == 2:
self.frontrightmethod()
elif self.dir == 3:
self.rightmethod()
else:
if self.dir == 0:
self.frontrightmethod()
elif self.dir == 1:
self.rightmethod()
elif self.dir == 2:
self.frontrightmethod()
elif self.dir == 3:
self.leftmethod()
elif self.mouse_x < 8:
if self.mouse_y < 8: # Micromouse位于左下角
if self.dir == 0:
self.frontrightmethod()
elif self.dir == 1:
self.frontleftmethod()
elif self.dir == 2:
self.leftmethod()
elif self.dir == 3:
self.rightmethod()
else: # Micromouse位于左上角
if self.dir == 0:
self.rightmethod()
elif self.dir == 1:
self.frontrightmethod()
elif self.dir == 2:
self.frontleftmethod()
elif self.dir == 3:
self.leftmethod()
else:
if self.mouse_y < 8: # Micromouse位于右下角
if self.dir == 0:
self.frontleftmethod()
elif self.dir == 1:
self.leftmethod()
elif self.dir == 2:
self.rightmethod()
elif self.dir == 3:
self.frontrightmethod()
else: # Micromouse位于右下角
if self.dir == 0:
self.leftmethod()
elif self.dir == 1:
self.rightmethod()
elif self.dir == 2:
self.frontrightmethod()
elif self.dir == 3:
self.frontleftmethod()
def destinationcheck(self):
if (self.mouse_x == 7 and self.mouse_y == 7) or (self.mouse_x == 7 and self.mouse_y == 8) or (
self.mouse_x == 8 and self.mouse_y == 7) or (self.mouse_x == 8 and self.mouse_y == 8):
return True
else:
return False
def crosswaychoice(self):
self.centralmethod()
def mapstepedit(self, dx, dy):
"""制作等高图, 禁止修改"""
self.mapstep = np.full((16, 16), 255)
step = 1
n = 1
stack = []
stack.append((dx, dy))
cx = dx
cy = dy
while n:
self.mapstep[cx, cy] = step
step += 1
count = 0
# 统计当前坐标有几个可前进的方向
if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] > step):
count += 1
if (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] > step):
count += 1
if (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] > step):
count += 1
if (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] > step):
count += 1
if count == 0:
cx, cy = stack.pop()
step = self.mapstep[cx, cy]
n -= 1
else:
if count > 1:
stack.append((cx, cy))
n += 1
# 随便挑一个方向，不影响结果，因为会全部走一遍
if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] > step):
cy += 1
continue
if (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] > step):
cx += 1
continue
if (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] > step):
cy -= 1
continue
if (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] > step):
cx -= 1
continue
def objectgoto(self, dx, dy):
"""向目标点运动, 禁止修改"""
self.mapstepedit(dx, dy)
temp_dir = 0
cx = self.mouse_x
cy = self.mouse_y
while (cx != dx) or (cy != dy):
# 沿着等高值减小的方向运行，直至到达目标点
step = self.mapstep[cx, cy]
if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] < step):
temp_dir = 0
elif (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] < step):
temp_dir = 1
elif (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] < step):
temp_dir = 2
elif (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] < step):
temp_dir = 3
d_dir = (temp_dir - self.dir + 4) % 4
if d_dir == 1:
self.turnright()
elif d_dir == 2:
self.turnback()
elif d_dir == 3:
self.turnleft()
self.moveoneblock()
cx = self.mouse_x
cy = self.mouse_y
def crosswaycheck(self):
temp = 0
left, down, right, up = self.mapblock[self.mouse_x, self.mouse_y][1:]
if up == '1':
if self.mapblock[self.mouse_x, self.mouse_y + 1][0] == '0':
temp += 1
if right == '1':
if self.mapblock[self.mouse_x + 1, self.mouse_y][0] == '0':
temp += 1
if down == '1':
if self.mapblock[self.mouse_x, self.mouse_y - 1][0] == '0':
temp += 1
if left == '1':
if self.mapblock[self.mouse_x - 1, self.mouse_y][0] == '0':
temp += 1
return temp
def mazesearch(self):
while 1:
# print(self.mouse_x, self.mouse_y)
if self.destinationcheck(): # 如果到了终点，则返回起点
destination = (self.mouse_x, self.mouse_y)
self.turnback()
self.objectgoto(0, 0)
return (1, destination)
else: # 否则一直搜索
crosswaycount = self.crosswaycheck()
if crosswaycount:
if crosswaycount > 1:
self.crosswaystack.append((self.mouse_x, self.mouse_y))
self.crosswaychoice()
self.moveoneblock()
if crosswaycount == 1:
self.crosswaychoice()
self.moveoneblock()
else:
self.turnback()
self.objectgoto(*self.crosswaystack.pop())
def spurt(self, dest):
self.objectgoto(*dest)
self.turnback()
self.objectgoto(0, 0)
def main():
rclpy.init(args=None)
micromouse = Micromouse()
t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
t.start()
flag, destination = micromouse.mazesearch()
micromouse.spurt(destination)
def main2():
""" remote control """
flag = -1
pygame.init()
if pygame.joystick.get_count():
stick = pygame.joystick.Joystick(0)
else:
print('No joystick found!!!')
rclpy.init(args=None)
micromouse = Micromouse()
t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
t.start()
while 1:
events = pygame.event.get()
if events:
for event in events:
if event.type == pygame.JOYBUTTONDOWN:
if event.button == 0:
flag = 0
print('go')
if event.button == 1:
flag = 1
print('turn right')
if event.button == 2:
flag = 2
print('turn back')
if event.button == 3:
flag = 3
print('turn left')
if event.button == 4:
flag = 66
print('go back')
if event.button == 5:
flag = 88
print('spurt')
if event.type == pygame.JOYBUTTONUP:
if event.button in (0, 1, 2, 3):
print('stop')
flag = -1
match flag:
case 0:
micromouse.moveoneblock()
case 1:
micromouse.turnright()
case 2:
micromouse.turnback()
case 3:
micromouse.turnleft()
case 66:
destination = (micromouse.mouse_x, micromouse.mouse_y)
micromouse.turnback()
micromouse.objectgoto(0, 0)
flag = -1
case 88:
print(destination)
micromouse.spurt(destination)
break
if __name__ == '__main__':
try:
main()
finally:
rclpy.shutdown()

579
src/windows/control/controltools/contest.py
Unescape View File

@ -116,20 +116,26 @@ class Drive(object):
return self.position_x, self.position_y, self.yaw
def posture_adjust(self):
fl_dis = self.fl_dis
fr_dis = self.fr_dis
l_dis = self.l_dis
r_dis = self.r_dis
if self.f_dis > 0.09:
if self.fl_dis < 0.118:
speed_z_temp = 0.5 * (self.fl_dis - 0.118) * self.kp
elif self.fr_dis < 0.118:
speed_z_temp = -0.5 * (self.fr_dis - 0.118) * self.kp
if fl_dis < 0.118:
speed_z_temp = 0.5 * (fl_dis - 0.118) * self.kp
elif fr_dis < 0.118:
speed_z_temp = -0.5 * (fr_dis - 0.118) * self.kp
else:
speed_z_temp = 0.0
else:
if self.l_dis < 0.085:
speed_z_temp = 0.5 * (self.l_dis - 0.085) * self.kp
elif self.r_dis < 0.085:
speed_z_temp = -0.5 * (self.r_dis - 0.085) * self.kp
if l_dis < 0.085:
speed_z_temp = 0.5 * (l_dis - 0.085) * self.kp
elif r_dis < 0.085:
speed_z_temp = -0.5 * (r_dis - 0.085) * self.kp
else:
speed_z_temp = 0.0
return speed_z_temp
def move(self, numblock=1):
@ -380,60 +386,55 @@ class Drive(object):
class Micromouse(Drive):
def __init__(self):
"""Inherits from the parent class, do not remove, but you can add your own content."""
"""继承父类，禁止删减，但可以增加自己要添加的内容。"""
super().__init__()
self.values_init()
def values_init(self):
"""Micromouse parameter initialization, do not modify."""
# 2D array to store wall information:
# flag: Whether visited, 0 or 1
# left: Whether there is a path, 0 or 1
# down: Whether there is a path, 0 or 1
# right: Whether there is a path, 0 or 1
# up: Whether there is a path, 0 or 1
self.mapblock = np.full((16, 16), '00000')
self.mapblock[0, 0] = '10001' # Initialize starting wall
self.valuesinit()
def valuesinit(self):
"""Micromosue参数初始化, 禁止修改"""
self.mapblock = np.full((16, 16), '00000') # 2D array,墙壁资料初始化, flag left down right up
# flag: 是否走过, 0 or 1
# left: 是否有路, 0 or 1
# down: 是否有路, 0 or 1
# right: 是否有路, 0 or 1
# up: 是否有路, 0 or 1
self.mapblock[0, 0] = '10001' # 起点墙壁初始化
self.crosswaystack = []
self.mouse_x = 0 # Starting coordinates in the maze
self.mouse_x = 0 # x,y起点坐标，这是在迷宫中的坐标
self.mouse_y = 0
self.dir = 0 # Micromouse heading: 0: up, 1: right, 2: down, 3: left
self.dir = 0 # Micromouse车头,0:up, 1:right, 2:down, 3:left
def coordinate_update(self):
"""Update coordinates based on the heading direction, do not modify."""
if self.dir == 0: # Heading up
def __coordinateupdate(self):
"""根据车头方向更新坐标, 禁止修改"""
if self.dir == 0: # 车头向上
self.mouse_y += 1
elif self.dir == 1: # Heading right
elif self.dir == 1: # 车头向右
self.mouse_x += 1
elif self.dir == 2: # Heading down
elif self.dir == 2: # 车头向下
self.mouse_y -= 1
elif self.dir == 3: # Heading left
elif self.dir == 3: # 车头向左
self.mouse_x -= 1
def get_coordinate(self):
"""Get coordinates for easy access, do not modify."""
"""获取坐标，方便调用，禁止修改"""
return self.mouse_x, self.mouse_y
def save_wall_info(self):
"""
Save wall information.
Do not modify this function.
"""
# Determine if there is a path in each direction based on distance sensors
front_path = '1' if self.f_dis > 0.2 else '0'
right_path = '1' if self.r_dis > 0.2 else '0'
back_path = '1'
left_path = '1' if self.l_dis > 0.2 else '0'
# Assign wall information based on the current direction
def __savewallinfo(self):
"""墙壁信息保存, 禁止修改"""
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
# 墙壁资料赋值
if self.dir == 0:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', left_path, back_path, right_path, front_path))
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', leftpath, backpath, rightpath, frontpath))
elif self.dir == 1:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', back_path, right_path, front_path, left_path))
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', backpath, rightpath, frontpath, leftpath))
elif self.dir == 2:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', right_path, front_path, left_path, back_path))
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', rightpath, frontpath, leftpath, backpath))
elif self.dir == 3:
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', front_path, left_path, back_path, right_path))
self.mapblock[self.mouse_x, self.mouse_y] = ''.join(('1', frontpath, leftpath, backpath, rightpath))
def turnright(self):
"""右转弯，加入了方向转换, 禁止修改"""
@ -450,36 +451,85 @@ class Micromouse(Drive):
self.dir = (self.dir + 2) % 4
return super(Micromouse, self).turnback()
def move_one_block(self):
"""
Move one block, then update coordinates, save wall information, and publish ROS custom message.
Do not modify this function.
"""
def moveoneblock(self):
"""移动一个单元格，然后更新坐标，保存墙壁信息的功能, 禁止修改"""
self.move()
self.update_coordinates()
if self.map_block[self.mouse_x, self.mouse_y][0] == '0':
self.save_wall_info()
self.publish_ros_custom()
self.__coordinateupdate()
if self.mapblock[self.mouse_x, self.mouse_y][0] == '0':
self.__savewallinfo()
self.ros_pub_custom()
def publish_ros_custom(self):
"""
Publish custom ROS message containing mouse coordinates, direction, and wall information.
Do not modify this function.
"""
def ros_pub_custom(self):
data = ''.join((str(self.mouse_x), ',',
str(self.mouse_y), ',',
str(self.dir), ',',
self.map_block[self.mouse_x, self.mouse_y][1:]
self.mapblock[self.mouse_x, self.mouse_y][1:]
))
self.string.data = data
self.pub_custom.publish(self.string)
def check_path(self, direction, distance_threshold=0.2):
"""
Check if there's a path in the specified direction with a given distance threshold.
Return True if path is clear, otherwise False.
"""
if direction == 'front':
def rightmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
def leftmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
@ -488,150 +538,240 @@ class Micromouse(Drive):
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif direction == 'right':
if flag == '0':
return
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x][self.mouse_y - 1][0]
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x][self.mouse_y + 1][0]
elif direction == 'left':
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
def frontrightmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x][self.mouse_y + 1][0]
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
def frontleftmethod(self):
frontpath = '1' if self.f_dis > 0.2 else '0'
rightpath = '1' if self.r_dis > 0.2 else '0'
backpath = '1'
leftpath = '1' if self.l_dis > 0.2 else '0'
if frontpath == '1': # 前方有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x][self.mouse_y - 1][0]
return flag == '0' and self.get_distance(direction) > distance_threshold
def right_method(self):
if self.check_path('right'):
self.turn_right()
elif self.check_path('front'):
pass
elif self.check_path('left'):
self.turn_left()
def left_method(self):
if self.check_path('left'):
self.turn_left()
elif self.check_path('front'):
pass
elif self.check_path('right'):
self.turn_right()
def front_right_method(self):
if self.check_path('front'):
pass
elif self.check_path('right'):
self.turn_right()
elif self.check_path('left'):
self.turn_left()
def front_left_method(self):
if self.check_path('front'):
pass
elif self.check_path('left'):
self.turn_left()
elif self.check_path('right'):
self.turn_right()
def central_method(self):
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
if flag == '0':
return
if leftpath == '1': # 左侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
if flag == '0':
self.turnleft()
return
if rightpath == '1': # 右侧有路
if self.dir == 0:
flag = self.mapblock[self.mouse_x + 1, self.mouse_y][0]
elif self.dir == 1:
flag = self.mapblock[self.mouse_x, self.mouse_y - 1][0]
elif self.dir == 2:
flag = self.mapblock[self.mouse_x - 1, self.mouse_y][0]
elif self.dir == 3:
flag = self.mapblock[self.mouse_x, self.mouse_y + 1][0]
if flag == '0':
self.turnright()
return
def centralmethod(self):
if self.mouse_x < 8:
if self.mouse_y < 8: # Micromouse位于左下角
if self.dir == 0:
self.frontrightmethod()
elif self.dir == 1:
self.frontleftmethod()
elif self.dir == 2:
self.leftmethod()
elif self.dir == 3:
self.rightmethod()
else: # Micromouse位于左上角
if self.dir == 0:
self.rightmethod()
elif self.dir == 1:
self.frontrightmethod()
elif self.dir == 2:
self.frontleftmethod()
elif self.dir == 3:
self.leftmethod()
else:
if self.mouse_y < 8: # Micromouse位于右下角
if self.dir == 0:
self.frontleftmethod()
elif self.dir == 1:
self.leftmethod()
elif self.dir == 2:
self.rightmethod()
elif self.dir == 3:
self.frontrightmethod()
else: # Micromouse位于右下角
if self.dir == 0:
self.leftmethod()
elif self.dir == 1:
self.rightmethod()
elif self.dir == 2:
self.frontrightmethod()
elif self.dir == 3:
self.frontleftmethod()
def centralmethod2(self):
if self.mouse_x in (7, 8) or self.mouse_y in (7, 8):
if self.mouse_y in (7, 8):
if self.mouse_x < 8:
if self.dir == 0:
self.right_method()
self.rightmethod()
elif self.dir == 1:
self.front_right_method()
self.frontrightmethod()
elif self.dir == 2:
self.left_method()
self.leftmethod()
elif self.dir == 3:
self.front_right_method()
self.frontrightmethod()
else:
if self.dir == 0:
self.left_method()
self.leftmethod()
elif self.dir == 1:
self.front_right_method()
self.frontrightmethod()
elif self.dir == 2:
self.right_method()
self.rightmethod()
elif self.dir == 3:
self.front_right_method()
self.frontrightmethod()
if self.mouse_x in (7, 8):
if self.mouse_y < 8:
if self.dir == 0:
self.front_right_method()
self.frontrightmethod()
elif self.dir == 1:
self.left_method()
self.leftmethod()
elif self.dir == 2:
self.front_right_method()
self.frontrightmethod()
elif self.dir == 3:
self.right_method()
self.rightmethod()
else:
if self.dir == 0:
self.front_right_method()
self.frontrightmethod()
elif self.dir == 1:
self.right_method()
self.rightmethod()
elif self.dir == 2:
self.front_right_method()
self.frontrightmethod()
elif self.dir == 3:
self.left_method()
self.leftmethod()
elif self.mouse_x < 8:
if self.mouse_y < 8: # Micromouse位于左下角
if self.dir == 0:
self.frontrightmethod()
elif self.dir == 1:
self.frontleftmethod()
elif self.dir == 2:
self.leftmethod()
elif self.dir == 3:
self.rightmethod()
else: # Micromouse位于左上角
if self.dir == 0:
self.rightmethod()
elif self.dir == 1:
self.frontrightmethod()
elif self.dir == 2:
self.frontleftmethod()
elif self.dir == 3:
self.leftmethod()
else:
if self.mouse_x < 8:
if self.mouse_y < 8:
if self.dir == 0:
self.front_right_method()
elif self.dir == 1:
self.front_left_method()
elif self.dir == 2:
self.left_method()
elif self.dir == 3:
self.right_method()
else:
if self.dir == 0:
self.right_method()
elif self.dir == 1:
self.front_right_method()
elif self.dir == 2:
self.front_left_method()
elif self.dir == 3:
self.left_method()
else:
if self.mouse_y < 8:
if self.dir == 0:
self.front_left_method()
elif self.dir == 1:
self.left_method()
elif self.dir == 2:
self.right_method()
elif self.dir == 3:
self.front_right_method()
else:
if self.dir == 0:
self.left_method()
elif self.dir == 1:
self.right_method()
elif self.dir == 2:
self.front_right_method()
elif self.dir == 3:
self.front_left_method()
def destination_check(self):
"""Check if the mouse is at one of the destination points."""
destination_points = [(7, 7), (7, 8), (8, 7), (8, 8)]
return (self.mouse_x, self.mouse_y) in destination_points
if self.mouse_y < 8: # Micromouse位于右下角
if self.dir == 0:
self.frontleftmethod()
elif self.dir == 1:
self.leftmethod()
elif self.dir == 2:
self.rightmethod()
elif self.dir == 3:
self.frontrightmethod()
else: # Micromouse位于右下角
if self.dir == 0:
self.leftmethod()
elif self.dir == 1:
self.rightmethod()
elif self.dir == 2:
self.frontrightmethod()
elif self.dir == 3:
self.frontleftmethod()
def destinationcheck(self):
if (self.mouse_x == 7 and self.mouse_y == 7) or (self.mouse_x == 7 and self.mouse_y == 8) or (
self.mouse_x == 8 and self.mouse_y == 7) or (self.mouse_x == 8 and self.mouse_y == 8):
return True
else:
return False
def crossway_choice(self):
"""Make a choice at a crossway."""
self.central_method()
def crosswaychoice(self):
self.centralmethod()
def map_step_edit(self, dx, dy):
"""Create a contour map, not to be modified."""
self.map_step = np.full((16, 16), 255)
def mapstepedit(self, dx, dy):
"""制作等高图, 禁止修改"""
self.mapstep = np.full((16, 16), 255)
step = 1
n = 1
stack = []
@ -640,39 +780,39 @@ class Micromouse(Drive):
cy = dy
while n:
self.map_step[cx, cy] = step
self.mapstep[cx, cy] = step
step += 1
count = 0
# Count available directions at the current coordinate
if (self.map_block[cx, cy][-1] == '1') and (self.map_step[cx, cy + 1] > step):
# 统计当前坐标有几个可前进的方向
if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] > step):
count += 1
if (self.map_block[cx, cy][-2] == '1') and (self.map_step[cx + 1, cy] > step):
if (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] > step):
count += 1
if (self.map_block[cx, cy][-3] == '1') and (self.map_step[cx, cy - 1] > step):
if (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] > step):
count += 1
if (self.map_block[cx, cy][-4] == '1') and (self.map_step[cx - 1, cy] > step):
if (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] > step):
count += 1
if count == 0:
cx, cy = stack.pop()
step = self.map_step[cx, cy]
step = self.mapstep[cx, cy]
n -= 1
else:
if count > 1:
stack.append((cx, cy))
n += 1
# Choose a direction, as all will be traversed
if (self.map_block[cx, cy][-1] == '1') and (self.map_step[cx, cy + 1] > step):
# 随便挑一个方向，不影响结果，因为会全部走一遍
if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] > step):
cy += 1
continue
if (self.map_block[cx, cy][-2] == '1') and (self.map_step[cx + 1, cy] > step):
if (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] > step):
cx += 1
continue
if (self.map_block[cx, cy][-3] == '1') and (self.map_step[cx, cy - 1] > step):
if (self.mapblock[cx, cy][-3] == '1') and (self.mapstep[cx, cy - 1] > step):
cy -= 1
continue
if (self.map_block[cx, cy][-4] == '1') and (self.map_step[cx - 1, cy] > step):
if (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] > step):
cx -= 1
continue
@ -685,8 +825,6 @@ class Micromouse(Drive):
while (cx != dx) or (cy != dy):
# 沿着等高值减小的方向运行，直至到达目标点
step = self.mapstep[cx, cy]
# 检查周围是否有墙，如果没有则更新方向
if (self.mapblock[cx, cy][-1] == '1') and (self.mapstep[cx, cy + 1] < step):
temp_dir = 0
elif (self.mapblock[cx, cy][-2] == '1') and (self.mapstep[cx + 1, cy] < step):
@ -695,7 +833,6 @@ class Micromouse(Drive):
temp_dir = 2
elif (self.mapblock[cx, cy][-4] == '1') and (self.mapstep[cx - 1, cy] < step):
temp_dir = 3
d_dir = (temp_dir - self.dir + 4) % 4
if d_dir == 1:
self.turnright()
@ -703,7 +840,6 @@ class Micromouse(Drive):
self.turnback()
elif d_dir == 3:
self.turnleft()
self.moveoneblock()
cx = self.mouse_x
cy = self.mouse_y
@ -711,8 +847,6 @@ class Micromouse(Drive):
def crosswaycheck(self):
temp = 0
left, down, right, up = self.mapblock[self.mouse_x, self.mouse_y][1:]
# 检查周围是否有路，如果有则计数
if up == '1':
if self.mapblock[self.mouse_x, self.mouse_y + 1][0] == '0':
temp += 1
@ -725,20 +859,17 @@ class Micromouse(Drive):
if left == '1':
if self.mapblock[self.mouse_x - 1, self.mouse_y][0] == '0':
temp += 1
return temp
def mazesearch(self):
while 1:
# print(self.mouse_x, self.mouse_y)
if self.destinationcheck():
# 如果到了终点，则返回起点
if self.destinationcheck(): # 如果到了终点，则返回起点
destination = (self.mouse_x, self.mouse_y)
self.turnback()
self.objectgoto(0, 0)
return (1, destination)
else:
# 否则一直搜索
else: # 否则一直搜索
crosswaycount = self.crosswaycheck()
if crosswaycount:
if crosswaycount > 1:
@ -753,14 +884,12 @@ class Micromouse(Drive):
self.objectgoto(*self.crosswaystack.pop())
def spurt(self, dest):
"""快速到达目的地并返回起点"""
self.objectgoto(*dest)
self.turnback()
self.objectgoto(0, 0)
def main():
"""主程序1 - 自动模式"""
rclpy.init(args=None)
micromouse = Micromouse()
t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
@ -770,41 +899,40 @@ def main():
def main2():
"""主程序2 - 遥控模式"""
""" remote control """
flag = -1
pygame.init()
stick = pygame.joystick.Joystick(0) if pygame.joystick.get_count() else None
if not stick:
if pygame.joystick.get_count():
stick = pygame.joystick.Joystick(0)
else:
print('No joystick found!!!')
rclpy.init(args=None)
micromouse = Micromouse()
t = threading.Thread(None, target=micromouse.ros_spin, daemon=True)
t.start()
while 1:
events = pygame.event.get()
if events:
for event in events:
if event.type == pygame.JOYBUTTONDOWN:
button_actions = {
0: ('go', micromouse.moveoneblock),
1: ('turn right', micromouse.turnright),
2: ('turn back', micromouse.turnback),
3: ('turn left', micromouse.turnleft),
4: ('go back', micromouse.goBack),
5: ('spurt', micromouse.spurt)
}
button = event.button
if button in button_actions:
flag = button
print(button_actions[button][0])
button_actions[button][1]()
else:
print(f'Button {button} has no action assigned.')
if event.button == 0:
flag = 0
print('go')
if event.button == 1:
flag = 1
print('turn right')
if event.button == 2:
flag = 2
print('turn back')
if event.button == 3:
flag = 3
print('turn left')
if event.button == 4:
flag = 66
print('go back')
if event.button == 5:
flag = 88
print('spurt')
if event.type == pygame.JOYBUTTONUP:
if event.button in (0, 1, 2, 3):
@ -812,6 +940,15 @@ def main2():
flag = -1
match flag:
case 0:
micromouse.moveoneblock()
case 1:
micromouse.turnright()
case 2:
micromouse.turnback()
case 3:
micromouse.turnleft()
case 66:
destination = (micromouse.mouse_x, micromouse.mouse_y)
micromouse.turnback()

BIN
src/windows/entity/operations/__pycache__/operation.cpython-311.pyc
View File

Binary file not shown.

91
src/windows/entity/operations/operation.py
Unescape View File

@ -0,0 +1,91 @@
import time
import pyautogui as p
from windows.res.tools.deal_picture import DealPicture
class operation:
def click_once(photo):
positions = p.locateCenterOnScreen(photo, confidence=0.8)
x, y = positions.x, positions.y
p.click(x, y)
time.sleep(0.1)
def click_util(click_photo, stop_function):
count = 0
while stop_function() is False:
positions = DealPicture.find_photo_center(click_photo)
if positions:
x, y = positions.x, positions.y
p.click(x, y)
else:
count += 1
if count == 20:
break
time.sleep(0.1)
def drag_once(drop_position, xOffset=None, yOffset=None, pOffsetX=None, pOffsetY=None):
offsetx = 0
offsety = 0
poffsetx = 0
poffsety = 0
if xOffset:
offsetx = xOffset
if yOffset:
offsety = yOffset
if pOffsetX:
poffsetx = pOffsetX
if pOffsetY:
poffsety = pOffsetY
positions = p.locateCenterOnScreen(drop_position, confidence=0.8)
if positions:
x, y = positions.x, positions.y
# 长按鼠标左键
p.mouseDown(x=x + poffsetx, y=y + poffsety, button='left')
# 向上移动鼠标，这里移动了100像素，可以根据需要调整
p.moveRel(offsetx, offsety, duration=1)
# 延迟一段时间
time.sleep(0.2)
# 松开鼠标左键
p.mouseUp(x=x + poffsetx + offsetx, y=y + poffsety + offsety, button='left')
time.sleep(0.3)
def drag_util(drop_position, stop_function, xOffset=None, yOffset=None, pOffsetX=None, pOffsetY=None):
count = 0
offsetx = 0
offsety = 0
poffsetx = 0
poffsety = 0
if xOffset:
offsetx = xOffset
if yOffset:
offsety = yOffset
if pOffsetX:
poffsetx = pOffsetX
if pOffsetY:
poffsety = pOffsetY
while stop_function() is False:
positions = DealPicture.find_photo_center(drop_position, 3)
if positions:
x, y = positions.x, positions.y
# 长按鼠标左键
p.mouseDown(x=x + poffsetx, y=y + poffsety, button='left')
# 向上移动鼠标，这里移动了100像素，可以根据需要调整
p.moveRel(offsetx, offsety, duration=1)
# 延迟一段时间
time.sleep(0.2)
# 松开鼠标左键
p.mouseUp(x=x + poffsetx + offsetx, y=y + poffsety + offsety, button='left')
time.sleep(0.3)
else:
count += 1
if count == 10:
break
time.sleep(0.3)

5
src/windows/main_window.py
Unescape View File

@ -20,6 +20,7 @@ class MyApp(QMainWindow):
self.shotArea = None
self.regPicture = None
self.translate_state = False
self.selected_language = "en"
self.translate_window = translate()
self.setRule_window = settingrule()
@ -132,11 +133,11 @@ class MyApp(QMainWindow):
def selectTransLanguage(self, index): # 各种语言选项的返回值
selected_language = self.language.itemText(index)
if selected_language == "日语":
print("jap")
print("jpn")
elif selected_language == "法语":
print("fra")
elif selected_language == "英语":
print("eng")
print("en")
if __name__ == "__main__":

BIN
src/windows/res/rules/__pycache__/明日方舟自动清体力.cpython-311.pyc
View File

Binary file not shown.

116
src/windows/res/rules/明日方舟自动清体力.py
Unescape View File

@ -5,97 +5,13 @@ import cv2
import pyautogui as p
from PyQt5.QtCore import QThread, pyqtSignal
from windows.entity.operations.operation import operation
from windows.res.tools.deal_picture import DealPicture
p.PAUSE = 0.1
p.FAILSAFE = True
def click_once(photo):
positions = p.locateCenterOnScreen(photo, confidence=0.8)
x, y = positions.x, positions.y
p.click(x, y)
time.sleep(0.1)
def click_util(click_photo, stop_function):
count = 0
while stop_function() is False:
positions = DealPicture.find_photo_center(click_photo)
if positions:
x, y = positions.x, positions.y
p.click(x, y)
else:
count += 1
if count == 20:
break
time.sleep(0.1)
def drag_once(drop_position, xOffset=None, yOffset=None, pOffsetX=None, pOffsetY=None):
offsetx = 0
offsety = 0
poffsetx = 0
poffsety = 0
if xOffset:
offsetx = xOffset
if yOffset:
offsety = yOffset
if pOffsetX:
poffsetx = pOffsetX
if pOffsetY:
poffsety = pOffsetY
positions = p.locateCenterOnScreen(drop_position, confidence=0.8)
if positions:
x, y = positions.x, positions.y
# 长按鼠标左键
p.mouseDown(x=x + poffsetx, y=y + poffsety, button='left')
# 向上移动鼠标，这里移动了100像素，可以根据需要调整
p.moveRel(offsetx, offsety, duration=1)
# 延迟一段时间
time.sleep(0.2)
# 松开鼠标左键
p.mouseUp(x=x + poffsetx + offsetx, y=y + poffsety + offsety, button='left')
time.sleep(0.3)
def drag_util(drop_position, stop_function, xOffset=None, yOffset=None, pOffsetX=None, pOffsetY=None):
count = 0
offsetx = 0
offsety = 0
poffsetx = 0
poffsety = 0
if xOffset:
offsetx = xOffset
if yOffset:
offsety = yOffset
if pOffsetX:
poffsetx = pOffsetX
if pOffsetY:
poffsety = pOffsetY
while stop_function() is False:
positions = DealPicture.find_photo_center(drop_position, 5)
if positions:
x, y = positions.x, positions.y
# 长按鼠标左键
p.mouseDown(x=x + poffsetx, y=y + poffsety, button='left')
# 向上移动鼠标，这里移动了100像素，可以根据需要调整
p.moveRel(offsetx, offsety, duration=1)
# 延迟一段时间
time.sleep(0.2)
# 松开鼠标左键
p.mouseUp(x=x + poffsetx + offsetx, y=y + poffsety + offsety, button='left')
time.sleep(0.3)
else:
count += 1
if count == 10:
break
time.sleep(0.3)
def backToTerminal():
print("back to terminal")
positions = DealPicture.find_photo_center('../pictures/terminal_photo.png', 2)
@ -103,11 +19,11 @@ def backToTerminal():
click_photo = cv2.imread('../pictures/back_btn.png')
back_confirm = cv2.imread('../pictures/back_confirm.png')
if DealPicture.find_photo_center(click_photo, 1):
click_once(click_photo)
operation.click_once(click_photo)
else:
return False
if DealPicture.find_photo_center(back_confirm, 1):
click_once(back_confirm)
operation.click_once(back_confirm)
else:
return False
positions = DealPicture.find_photo_center('../pictures/terminal_photo.png')
@ -130,7 +46,7 @@ def find_target_level():
drag_position = cv2.imread('../pictures/back_btn.png')
name = 'level_1-7.png'
if DealPicture.find_photo_center('../pictures/back_btn.png', 2):
drag_once(drag_position, 300, 0, 0, 200)
operation.drag_once(drag_position, 300, 0, 0, 200)
else:
print("don't find picture ", name)
return False
@ -138,7 +54,7 @@ def find_target_level():
while DealPicture.find_photo_center('../pictures/level_1-7.png', 1, 0.9) is None:
drag_position = cv2.imread('../pictures/back_btn.png')
if DealPicture.find_photo_center('../pictures/back_btn.png', 2):
drag_once(drag_position, -400, 0, 300, 200)
operation.drag_once(drag_position, -400, 0, 300, 200)
else:
print("back to terminal")
return False
@ -147,7 +63,7 @@ def find_target_level():
stop_photo = [cv2.imread('../pictures/fight_photo.png')]
name = 'level_1-7.png'
if DealPicture.find_photo_center(click_photo, 1, 0.9):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
else:
print("don't find picture", name)
return False
@ -160,7 +76,7 @@ def find_target_chapter():
stop_photo = [cv2.imread('../pictures/theme_word.png')]
name = 'terminal_photo.png'
if DealPicture.find_photo_center(click_photo, 1):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
else:
print("don't find picture", name)
return False
@ -169,7 +85,7 @@ def find_target_chapter():
stop_photo = [cv2.imread('../pictures/black_circle.png')]
name = 'theme_word.png'
if DealPicture.find_photo_center(click_photo, 1):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
else:
print("don't find picture", name)
return False
@ -178,7 +94,7 @@ def find_target_chapter():
stop_photo = [cv2.imread('../pictures/purpose_chapter.png')]
name = 'black_circle.png'
if DealPicture.find_photo_center(drag_position, 1):
drag_util(drag_position, lambda: DealPicture.find_any_photos(stop_photo), 0, 400)
operation.drag_util(drag_position, lambda: DealPicture.find_any_photos(stop_photo), 0, 400)
else:
print("don't find picture", name)
return False
@ -187,7 +103,7 @@ def find_target_chapter():
stop_photo = [cv2.imread('../pictures/chapter2.png')]
name = 'chapter3.png'
if DealPicture.find_photo_center(drag_position, 1):
drag_util(drag_position, lambda: DealPicture.find_any_photos(stop_photo), 300, 0)
operation.drag_util(drag_position, lambda: DealPicture.find_any_photos(stop_photo), 300, 0)
else:
print("don't find picture", name)
return False
@ -196,7 +112,7 @@ def find_target_chapter():
stop_photo = [cv2.imread('../pictures/chapter1.png')]
name = 'chapter2.png'
if DealPicture.find_photo_center(drag_position, 1):
drag_util(drag_position, lambda: DealPicture.find_any_photos(stop_photo), 300, 0)
operation.drag_util(drag_position, lambda: DealPicture.find_any_photos(stop_photo), 300, 0)
else:
print("don't find picture", name)
return False
@ -205,7 +121,7 @@ def find_target_chapter():
stop_photos = [cv2.imread('../pictures/fight_photo.png'), cv2.imread('../pictures/cur_chapter1.png')]
name = 'chapter1.png'
if DealPicture.find_photo_center(click_photo, 1):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photos))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photos))
else:
print("don't find picture", name)
return False
@ -219,7 +135,7 @@ def fight_time():
stop_photo = [cv2.imread('../pictures/start_fighting.png')]
name = 'fight_photo.png'
if DealPicture.find_photo_center(click_photo, 1):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
else:
print("don't find picture", name)
return False
@ -228,7 +144,7 @@ def fight_time():
stop_photo = [cv2.imread('../pictures/fighting!.png')]
name = 'start_fighting!.png'
if DealPicture.find_photo_center(click_photo, 2):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
else:
print("don't find picture", name)
return False
@ -241,13 +157,13 @@ def fight_time():
click_photo = cv2.imread('../pictures/mission_fail.png')
stop_photo = [cv2.imread('../pictures/mission_complete.png')]
if DealPicture.find_photo_center(click_photo, 5):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
click_photo = cv2.imread('../pictures/mission_complete.png')
stop_photo = [cv2.imread('../pictures/fight_photo.png')]
name = 'mission_complete.png'
if DealPicture.find_photo_center(click_photo, 5):
click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
operation.click_util(click_photo, lambda: DealPicture.find_any_photos(stop_photo))
else:
print("don't find picture", name)
return False

Write Preview
Loading…
Cancel
Save