[PYTHON] Faisons un robot qui résout le Rubik Cube! 3 Logiciel
Quel est cet article?
Cet article est sérialisé ** Faisons un robot qui résout le Rubik Cube! Il fait partie de la collection d'articles appelée **. Cliquez ici pour voir l'image complète
- Présentation
- Algorithme
- Logiciel (cet article)
- Matériel
GitHub est ici.
La vidéo promotionnelle est ici.
La collection de vidéos sur cette collection d'articles est ici.
Contenu de cet article
Dans cet article, je vais vous expliquer le logiciel de Soltvvo, un robot qui résout le cube rubic 2x2x2 que j'ai réalisé.
Constitution
Avant d'expliquer le logiciel, il est nécessaire de noter le type de configuration que le robot utilisera. Cela ressemble à la figure.
Ici, Raspberry Pi est un ordinateur monocarte appelé Raspberry Pi 4 (2 Go de RAM) (c'est-à-dire un ordinateur ordinaire (?)), Et ATMEGA328P est un célèbre micro-ordinateur (microcontrôleur), utilisé pour une carte micro-ordinateur appelée Arduino. Cette fois, je vais l'utiliser comme Arduino Uno. Il existe deux moteurs pas à pas et deux servomoteurs de ATMEGA328P. C'est l'actionneur (puissance) qui déplace réellement le bras qui fait tourner le cube. Vous pouvez également utiliser la webcam pour entrer l'état du cube.
L'image entière
Je vais vous montrer l'image complète du programme.
Aperçu du programme côté Raspberry Pi
- Une classe qui représente l'état du cube
- Gestion facile du statut CP et CO
- Description facile pour faire fonctionner le cube
- Fonction pour déplacer l'actionneur
- Envoyez une commande pour déplacer le bras
- GUI
- Écran d'affichage
- Obtenez l'état du puzzle à l'aide de la caméra
- Reconnaissance d'image
- Rechercher une solution
- IDA * Recherche
- Le pré-calcul est calculé à l'avance et transformé en fichier csv
- Faites une procédure pour que le robot se déplace de la liste des symboles de rotation
- Essayez d'obtenir la meilleure solution
- En fait, déplacez le robot
Aperçu du programme côté ATMEGA328P
- Recevoir la commande
- Fonction pour déplacer le moteur
- Fonction pour saisir le cube avec le bras
- Fonction pour libérer le cube avec le bras
Programme côté Raspberry Pi-Python
Ici, nous allons vous expliquer le programme côté PC. Le programme compte environ 580 lignes et est long, je vais donc l'expliquer en le divisant en plusieurs parties.
Importer la bibliothèque
Importez la bibliothèque que vous souhaitez utiliser.
from copy import deepcopy
from collections import deque
from time import time, sleep
import tkinter
import cv2
import numpy as np
import serial
Classe de cube
J'ai créé une classe Cube car il est bon de penser à un cube comme une «chose» dans le programme. J'ai honte de dire que c'est mon premier cours de ma vie.
class Cube:
def __init__(self):
self.Co = [0, 0, 0, 0, 0, 0, 0]
self.Cp = [0, 1, 2, 3, 4, 5, 6]
#Traitement de rotation CP
def move_cp(self, num):
surface = [[0, 1, 2, 3], [2, 3, 4, 5], [3, 1, 5, 6]]
replace = [[2, 0, 3, 1], [3, 2, 1, 0], [1, 3, 0, 2]]
idx = num // 3
res = [i for i in self.Cp]
for i, j in zip(surface[idx], replace[num % 3]):
res[i] = self.Cp[surface[idx][j]]
return res
#Traitement de rotation CO
def move_co(self, num):
surface = [[0, 1, 2, 3], [2, 3, 4, 5], [3, 1, 5, 6]]
replace = [[2, 0, 3, 1], [3, 2, 1, 0], [1, 3, 0, 2]]
pls = [2, 1, 1, 2]
idx = num // 3
res = [i for i in self.Co]
for i, j in zip(surface[idx], replace[num % 3]):
res[i] = self.Co[surface[idx][j]]
if num // 3 != 0 and num % 3 != 1:
for i in range(4):
res[surface[idx][i]] += pls[i]
res[surface[idx][i]] %= 3
return res
#Changez en fait la disposition des états du puzzle en fonction du numéro de rotation
def move(self, num):
res = Cube()
res.Co = self.move_co(num)
res.Cp = self.move_cp(num)
return res
#Créer un numéro unique à partir du tableau cp
def cp2i(self):
res = 0
marked = set([])
for i in range(7):
res += fac[6 - i] * len(set(range(self.Cp[i])) - marked)
marked.add(self.Cp[i])
return res
#Créer un numéro unique à partir du tableau co
def co2i(self):
res = 0
for i in self.Co:
res *= 3
res += i
return res
J'expliquerai les fonctions une par une. __init__(self)
def __init__(self):
self.Co = [0, 0, 0, 0, 0, 0, 0]
self.Cp = [0, 1, 2, 3, 4, 5, 6]
C'est la première fonction à être exécutée lorsque la classe Cube est appelée. Ici, deux tableaux, Co et Cp, sont associés à la classe Cube. Ce sont, dans l'ordre, CO (abréviation de Corner Orientation, qui indique l'orientation des pièces d'angle) et CP (abréviation de Corner Permutation, qui indique la position des pièces d'angle).
De plus, il est très efficace d'avoir votre propre séquence CO et séquence CP pour maintenir l'état du cube. De plus, comme il n'y a que R, U et F à tourner, les pièces DLB (en bas à gauche en arrière) ne bougent pas. Vous n'êtes pas obligé de conserver les informations pour cette partie, vous n'avez besoin que de 7 données pour chaque CO et CP.
CP numérote les parties comme indiqué et contient les valeurs de sorte que la partie à l'emplacement d'index de l'élément dans le tableau CP soit l'élément dans le tableau CP.
Le CO est 0 lorsque l'autocollant blanc ou jaune apparaît sur le côté U ou D, et devient 1 ou 2 chaque fois qu'il est tourné de 120 degrés dans le sens des aiguilles d'une montre à partir de là. À titre d'exemple, donnez un numéro de CO à la photo.
move_cp(self, num)
#Traitement de rotation CP
def move_cp(self, num):
surface = [[0, 1, 2, 3], [2, 3, 4, 5], [3, 1, 5, 6]]
replace = [[2, 0, 3, 1], [3, 2, 1, 0], [1, 3, 0, 2]]
idx = num // 3
res = [i for i in self.Cp]
for i, j in zip(surface[idx], replace[num % 3]):
res[i] = self.Cp[surface[idx][j]]
return res
Cette fonction est (une partie de) la fonction qui fait réellement tourner le cube. Faites pivoter uniquement le CP de la baie. Je ne plaisante pas avec CO. De plus, num représente le numéro du symbole de rotation (numéro de rotation), et correspond à l'index du tableau suivant placé globalement.
move_candidate = ["U", "U2", "U'", "F", "F2", "F'", "R", "R2", "R'"] #Candidat à la rotation
Le tableau de surface montre les pièces qui se déplacent lorsque les plans U, F et R sont déplacés, et le tableau de remplacement montre comment les pièces se déplacent lorsque X, X2 et X sont déplacés, respectivement. Le traitement de rotation est rendu possible en remplaçant correctement ces séquences.
move_co(self, num)
#Traitement de rotation CO
def move_co(self, num):
surface = [[0, 1, 2, 3], [2, 3, 4, 5], [3, 1, 5, 6]]
replace = [[2, 0, 3, 1], [3, 2, 1, 0], [1, 3, 0, 2]]
pls = [2, 1, 1, 2]
idx = num // 3
res = [i for i in self.Co]
for i, j in zip(surface[idx], replace[num % 3]):
res[i] = self.Co[surface[idx][j]]
if num // 3 != 0 and num % 3 != 1:
for i in range(4):
res[surface[idx][i]] += pls[i]
res[surface[idx][i]] %= 3
return res
Cette fonction, comme la fonction move_cp ci-dessus, est une fonction qui déplace réellement la pièce. Cette fonction pilote le CO. Le contenu est très similaire à la fonction move_cp, mais le CO change lorsque le plan U n'est pas tourné (num% 3! = 0) et quand il ne pivote pas de 180 degrés (num% 3! = 1), il doit donc être traité de manière appropriée. il y a.
move(self, num)
#Changez en fait la disposition des états du puzzle en fonction du numéro de rotation
def move(self, num):
res = Cube()
res.Co = self.move_co(num)
res.Cp = self.move_cp(num)
return res
Cette fonction est également une fonction qui déplace réellement la pièce. Cette fonction pilote à la fois CO et CP. J'exécute juste les deux fonctions ci-dessus.
cp2i(self)
#Créer un numéro unique à partir du tableau cp
def cp2i(self):
res = 0
marked = set([])
for i in range(7):
res += fac[6 - i] * len(set(range(self.Cp[i])) - marked)
marked.add(self.Cp[i])
return res
Cette fonction génère et renvoie un numéro spécifique au tableau CP à partir du tableau CP. Il renvoie simplement le numéro de séquence du tableau CP. Qu'est-ce qu'un numéro de séquence? 0: 0, 1, 2, 3, 4, 5, 6 1: 0, 1, 2, 3, 4, 6, 5 2: 0, 1, 2, 3, 5, 4, 6 3: 0, 1, 2, 3, 5, 6, 4 4: 0, 1, 2, 3, 6, 5, 4 C'est le nombre lorsqu'il est disposé dans l'ordre comme. Je pense que le site de ici est facile à comprendre comment calculer ce nombre.
co2i(self)
#Créer un numéro unique à partir du tableau co
def co2i(self):
res = 0
for i in self.Co:
res *= 3
res += i
return res
Cette fonction génère et renvoie un nombre spécifique au tableau CO à partir du tableau CO. À ce stade, un numéro unique est créé en traitant le tableau CO comme un nombre à 7 chiffres en nombres ternaires.
Fonctions liées à d'autres puzzles
Il existe une fonction appelée num2moves qui traduit les nombres de rotation en symboles de rotation.
#Convertir le numéro de rotation en symbole de rotation
def num2moves(arr):
res = ''
for i in arr:
res += move_candidate[i] + ' '
return res
move_candidate est
move_candidate = ["U", "U2", "U'", "F", "F2", "F'", "R", "R2", "R'"] #Candidat à la rotation
est.
Fonction pour déplacer l'actionneur
L'image entière est ici.
def move_actuator(num, arg1, arg2, arg3=None):
if arg3 == None:
com = str(arg1) + ' ' + str(arg2)
else:
com = str(arg1) + ' ' + str(arg2) + ' ' + str(arg3)
ser_motor[num].write((com + '\n').encode())
ser_motor[num].flush()
print('num:', num, 'command:', com)
def grab_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 5)
def release_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 6)
move_actuator(num, arg1, arg2, arg3=None)
#Envoyer une commande pour déplacer l'actionneur
def move_actuator(num, arg1, arg2, arg3=None):
if arg3 == None:
com = str(arg1) + ' ' + str(arg2)
else:
com = str(arg1) + ' ' + str(arg2) + ' ' + str(arg3)
ser_motor[num].write((com + '\n').encode())
ser_motor[num].flush()
print('num:', num, 'command:', com)
Cette fonction est une fonction qui envoie une commande à ATMEGA328P par communication série pour déplacer le moteur. Ici, num est le numéro ATMEGA328P (0 ou 1), arg1 est le numéro de l'actionneur, arg2 est la quantité de fonctionnement de l'actionneur et arg3 est la vitesse (tr / min) du moteur lors du déplacement du moteur pas à pas. ..
Concernant la communication série,
ser_motor[0] = serial.Serial('/dev/ttyUSB0', 9600, write_timeout=0)
ser_motor[1] = serial.Serial('/dev/ttyUSB1', 9600, write_timeout=0)
Il y a une définition. Chaque ATMEGA328P dispose de deux moteurs pas à pas et de deux servomoteurs.
grab_p()
#Prenez le cube
def grab_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 1000)
Cette fonction utilise la fonction move_actuator mentionnée précédemment pour déplacer tous les bras à la fois et saisir le cube.
release_p()
#Libérez le cube
def release_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 2000)
Cette fonction déplace tous les bras à la fois et libère le cube.
GUI La partie relative à l'interface graphique est ici.
root = tkinter.Tk()
root.title("2x2x2solver")
root.geometry("300x150")
grid = 20
offset = 30
entry = [[None for _ in range(8)] for _ in range(6)]
for i in range(6):
for j in range(8):
if 1 < i < 4 or 1 < j < 4:
entry[i][j] = tkinter.Entry(master=root, width=2, bg='gray')
entry[i][j].place(x = j * grid + offset, y = i * grid + offset)
inspection = tkinter.Button(root, text="inspection", command=inspection_p)
inspection.place(x=0, y=0)
start = tkinter.Button(root, text="start", command=start_p)
start.place(x=0, y=40)
solutionvar = tkinter.StringVar(master=root, value='')
solution = tkinter.Label(textvariable=solutionvar)
solution.place(x=70, y=0)
solvingtimevar = tkinter.StringVar(master=root, value='')
solvingtime = tkinter.Label(textvariable=solvingtimevar)
solvingtime.place(x=120, y=20)
grab = tkinter.Button(root, text="grab", command=grab_p)
grab.place(x=0, y=120)
release = tkinter.Button(root, text="release", command=release_p)
release.place(x=120, y=120)
root.mainloop()
J'utilise tkinter pour la bibliothèque GUI. J'expliquerai à quoi correspond chaque variable dans la figure.
Bien que cela ne soit pas montré ici, il existe une étiquette appelée temps de résolution qui montre à l'écran combien de temps il a fallu pour résoudre le cube.
La raison pour laquelle c'est la zone de saisie qui affiche la couleur est que ce programme est entré à l'origine dans l'état du cube en tapant la couleur dans la zone de saisie. Une telle époque est révolue et maintenant elle est reconnue par la caméra.
Stockez les couleurs du puzzle dans un tableau
L'image entière est ici.
dic = {'w':'white', 'g':'green', 'r':'red', 'b':'blue', 'o':'magenta', 'y':'yellow'}
parts_place = [[[0, 2], [2, 0], [2, 7]], [[0, 3], [2, 6], [2, 5]], [[1, 2], [2, 2], [2, 1]], [[1, 3], [2, 4], [2, 3]], [[4, 2], [3, 1], [3, 2]], [[4, 3], [3, 3], [3, 4]], [[5, 3], [3, 5], [3, 6]], [[5, 2], [3, 7], [3, 0]]]
parts_color = [['w', 'o', 'b'], ['w', 'b', 'r'], ['w', 'g', 'o'], ['w', 'r', 'g'], ['y', 'o', 'g'], ['y', 'g', 'r'], ['y', 'r', 'b'], ['y', 'b', 'o']]
#Refléter la couleur dans la boîte
def confirm_p():
global colors
for i in range(6):
for j in range(8):
if (1 < i < 4 or 1 < j < 4) and colors[i][j] in j2color:
entry[i][j]['bg'] = dic[colors[i][j]]
#Remplissez l'endroit où la couleur est fixée là où elle n'est pas remplie
for i in range(6):
for j in range(8):
if (1 < i < 4 or 1 < j < 4) and colors[i][j] == '':
done = False
for k in range(8):
if [i, j] in parts_place[k]:
for strt in range(3):
if parts_place[k][strt] == [i, j]:
idx = [colors[parts_place[k][l % 3][0]][parts_place[k][l % 3][1]] for l in range(strt + 1, strt + 3)]
for strt2 in range(3):
idx1 = strt2
idx2 = (strt2 + 1) % 3
idx3 = (strt2 + 2) % 3
for l in range(8):
if parts_color[l][idx1] == idx[0] and parts_color[l][idx2] == idx[1]:
colors[i][j] = parts_color[l][idx3]
entry[i][j]['bg'] = dic[colors[i][j]]
done = True
break
if done:
break
break
if done:
break
#Changer la couleur d'arrière-plan de la zone vide en gris
for i in range(6):
for j in range(8):
if (1 < i < 4 or 1 < j < 4) and colors[i][j] == '':
entry[i][j]['bg'] = 'gray'
Cette fonction est un peu compliquée. Pour 2x2x2, il suffit de ne regarder que les quatre côtés (bien choisis) du puzzle (par exemple, les côtés D, F, U et B dans ce programme) pour déterminer de manière unique l'état du puzzle. C'est donc cette fonction qui devine et détermine la couleur de la surface qui n'est pas vue. La couleur choisie sera reflétée comme la couleur d'arrière-plan de la zone de saisie.
Cette fonction effectue simplement une recherche complète pour savoir quelle sera la couleur non remplie. Cependant, la mise en œuvre est devenue un peu lourde. Je ne suis pas sûr que ce soit la meilleure façon de l'écrire.
Obtenez l'état du puzzle de la caméra
L'image entière est ici.
j2color = ['g', 'b', 'r', 'o', 'y', 'w']
idx = 0
colors = [['' for _ in range(8)] for _ in range(6)]
#Obtenez l'état du puzzle
def detect():
global idx, colors
idx = 0
while idx < 4:
#color: g, b, r, o, y, w
color_low = [[50, 50, 50], [90, 150, 50], [160, 150, 50], [170, 50, 50], [20, 50, 50], [0, 0, 50]] #for PC
color_hgh = [[90, 255, 255], [140, 255, 255], [10, 255, 200], [20, 255, 255], [50, 255, 255], [179, 50, 255]]
circlecolor = [(0, 255, 0), (255, 0, 0), (0, 0, 255), (0, 170, 255), (0, 255, 255), (255, 255, 255)]
surfacenum = [[[4, 2], [4, 3], [5, 2], [5, 3]], [[2, 2], [2, 3], [3, 2], [3, 3]], [[0, 2], [0, 3], [1, 2], [1, 3]], [[3, 7], [3, 6], [2, 7], [2, 6]]]
capture = cv2.VideoCapture(0)
ret, frame = capture.read()
capture.release()
size_x = 200
size_y = 150
frame = cv2.resize(frame, (size_x, size_y))
show_frame = deepcopy(frame)
d = 50
center = [size_x // 2, size_y // 2]
tmp_colors = [['' for _ in range(8)] for _ in range(6)]
hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
dx = [-1, 1, -1, 1]
dy = [1, 1, -1, -1]
for i in range(4):
y = center[0] + dy[i] * d
x = center[1] + dx[i] * d
cv2.circle(show_frame, (y, x), 5, (0, 0, 0), thickness=3, lineType=cv2.LINE_8, shift=0)
val = hsv[x, y]
for j in range(6):
flag = True
for k in range(3):
if not ((color_low[j][k] < color_hgh[j][k] and color_low[j][k] <= val[k] <= color_hgh[j][k]) or (color_low[j][k] > color_hgh[j][k] and (color_low[j][k] <= val[k] or val[k] <= color_hgh[j][k]))):
flag = False
if flag:
tmp_colors[surfacenum[idx][i][0]][surfacenum[idx][i][1]] = j2color[j]
cv2.circle(show_frame, (y, x), 15, circlecolor[j], thickness=3, lineType=cv2.LINE_8, shift=0)
break
cv2.imshow('title',show_frame)
key = cv2.waitKey(0)
if key == 32: #Lorsque la touche d'espace est enfoncée
for i in range(4):
colors[surfacenum[idx][i][0]][surfacenum[idx][i][1]] = tmp_colors[surfacenum[idx][i][0]][surfacenum[idx][i][1]]
print(idx)
idx += 1
confirm_p()
if idx < 4:
for i in range(2):
move_actuator(i, 0, 5)
move_actuator(i, 0, (-1) ** i, 50)
move_actuator(i, 1, 5)
move_actuator((i + 1) % 2, 1, 5)
move_actuator(i, 0, 6)
move_actuator(i, 0, -(-1) ** i, 50)
move_actuator(i, 0, 5)
move_actuator(i, 1, 6)
move_actuator((i + 1) % 2, 1, 6)
cv2.destroyAllWindows()
Cette fonction obtient la couleur du puzzle en fonction de la couleur de chacun des quatre pixels de l'image prise par la caméra. L'écran ressemble à ceci.
Créer une procédure spécifique au robot
Ce qui est produit à l'aide d'IDA * dans la fonction d'inspection décrite plus loin est une liste de symboles de rotation (numéros de rotation) pour la lecture humaine. Convertissez-le en un tableau contenant des informations sur le moteur à déplacer, quand et combien de fois. L'image entière est ici.
#Déterminer le moteur à tourner à partir de la disposition des numéros de symboles de rotation
def proc_motor(rot, num, direction):
if num == len(ans):
return rot, num, direction
turn_arr = [-3, -2, -1]
r_arr = [[-1, 2, 4, -1, 5, 1], [5, -1, 0, 2, -1, 3], [1, 3, -1, 4, 0, -1], [-1, 5, 1, -1, 2, 4], [2, -1, 3, 5, -1, 0], [4, 0, -1, 1, 3, -1]]
f_arr = [[1, 2, 4, 5], [3, 2, 0, 5], [3, 4, 0, 1], [4, 2, 1, 5], [3, 5, 0, 2], [3, 1, 0, 4]]
regrip_arr = [[21, 5, 9, 17, 20, 13, 10, 3, 4, 12, 18, 0, 23, 19, 11, 7, 8, 15, 22, 1, 16, 14, 6, 2], [4, 8, 16, 20, 12, 9, 2, 23, 15, 17, 3, 7, 18, 10, 6, 22, 14, 21, 0, 11, 13, 5, 1, 19]]
regrip_rot = [[[1, -3], [3, -1]], [[0, -3], [2, -1]]]
u_face = direction // 4
f_face = f_arr[u_face][direction % 4]
r_face = r_arr[u_face][f_face]
d_face = (u_face + 3) % 6
b_face = (f_face + 3) % 6
l_face = (r_face + 3) % 6
move_able = [r_face, b_face, l_face, f_face]
move_face = ans[num] // 3
move_amount = turn_arr[ans[num] % 3]
if move_face == u_face or move_face == d_face:
rot_tmp = [[i for i in rot] for _ in range(2)]
direction_tmp = [-1, -1]
num_tmp = [num, num]
for j in range(2):
rot_tmp[j].extend(regrip_rot[j])
direction_tmp[j] = regrip_arr[j][direction]
rot_tmp[j], num_tmp[j], direction_tmp[j] = proc_motor(rot_tmp[j], num_tmp[j], direction_tmp[j])
idx = 0 if len(rot_tmp[0]) < len(rot_tmp[1]) else 1
rot_res = rot_tmp[idx]
num_res = num_tmp[idx]
direction_res = direction_tmp[idx]
else:
tmp = move_able.index(move_face)
rot_res = [i for i in rot]
rot_res.append([tmp, move_amount])
rot_res, num_res, direction_res = proc_motor(rot_res, num + 1, direction)
return rot_res, num_res, direction_res
#Optimisation des procédures du robot
def rot_optimise():
global rot
i = 0
tmp_arr = [0, -3, -2, -1]
while i < len(rot):
if i < len(rot) - 1 and rot[i][0] == rot[i + 1][0]:
tmp = tmp_arr[(rot[i][1] + rot[i + 1][1]) % 4]
del rot[i + 1]
if not tmp:
del rot[i]
i -= 1
else:
rot[i][1] = tmp
elif i < len(rot) - 2 and rot[i][0] == rot[i + 2][0] and rot[i][0] % 2 == rot[i + 1][0] % 2:
tmp = tmp_arr[rot[i][1] + rot[i + 2][1] + 2]
del rot[i + 2]
if tmp == 0:
del rot[i]
i -= 1
else:
rot[i][1] = tmp
i += 1
On dirait que vous faites quelque chose de très stupide. Il existe un certain nombre de tableaux ridiculement longs fabriqués à la main. Expliquons les fonctions une par une.
proc_motor(rot, num, direction)
#Déterminer le moteur à tourner à partir de la disposition des numéros de symboles de rotation
def proc_motor(rot, num, direction):
if num == len(ans):
return rot, num, direction
turn_arr = [-3, -2, -1]
r_arr = [[-1, 2, 4, -1, 5, 1], [5, -1, 0, 2, -1, 3], [1, 3, -1, 4, 0, -1], [-1, 5, 1, -1, 2, 4], [2, -1, 3, 5, -1, 0], [4, 0, -1, 1, 3, -1]]
f_arr = [[1, 2, 4, 5], [3, 2, 0, 5], [3, 4, 0, 1], [4, 2, 1, 5], [3, 5, 0, 2], [3, 1, 0, 4]]
regrip_arr = [[21, 5, 9, 17, 20, 13, 10, 3, 4, 12, 18, 0, 23, 19, 11, 7, 8, 15, 22, 1, 16, 14, 6, 2], [4, 8, 16, 20, 12, 9, 2, 23, 15, 17, 3, 7, 18, 10, 6, 22, 14, 21, 0, 11, 13, 5, 1, 19]]
regrip_rot = [[[1, -3], [3, -1]], [[0, -3], [2, -1]]]
u_face = direction // 4
f_face = f_arr[u_face][direction % 4]
r_face = r_arr[u_face][f_face]
d_face = (u_face + 3) % 6
b_face = (f_face + 3) % 6
l_face = (r_face + 3) % 6
move_able = [r_face, b_face, l_face, f_face]
move_face = ans[num] // 3
move_amount = turn_arr[ans[num] % 3]
if move_face == u_face or move_face == d_face:
rot_tmp = [[i for i in rot] for _ in range(2)]
direction_tmp = [-1, -1]
num_tmp = [num, num]
for j in range(2):
rot_tmp[j].extend(regrip_rot[j])
direction_tmp[j] = regrip_arr[j][direction]
rot_tmp[j], num_tmp[j], direction_tmp[j] = proc_motor(rot_tmp[j], num_tmp[j], direction_tmp[j])
idx = 0 if len(rot_tmp[0]) < len(rot_tmp[1]) else 1
rot_res = rot_tmp[idx]
num_res = num_tmp[idx]
direction_res = direction_tmp[idx]
else:
tmp = move_able.index(move_face)
rot_res = [i for i in rot]
rot_res.append([tmp, move_amount])
rot_res, num_res, direction_res = proc_motor(rot_res, num + 1, direction)
return rot_res, num_res, direction_res
Je parlerai des détails dans la section du matériel, mais le robot a quatre bras, qui sont respectivement attachés aux côtés F, R, B et L. En d'autres termes, le côté U ne peut être tourné que si vous changez de main. Ici, la commutation revient à tourner le côté R et le côté L en même temps, par exemple. Il y a $ 4 (rues / faces) \ times6 (faces) = 24 façons de tenir le cube, alors attribuez un numéro à chaque direction et comment passer de chaque direction à quelle direction il sera. Dans le tableau regrip_arr (c'est un travail difficile). Il existe deux façons de basculer entre RL et FB en même temps (que ce soit dans le sens horaire ou antihoraire, les valeurs sont les mêmes après tout).
À partir de l'orientation actuelle du cube, nous utiliserons la recherche complète récursive pour savoir si l'étape suivante peut être tournée sans commutation, et si cela est nécessaire, laquelle des deux commutations est la plus efficace.
Les valeurs sont stockées dans le tableau rot comme indiqué ci-dessous.
rot = [[Nombre de moteur à tourner,Direction et degré de rotation], [0, -90], [1, -180], [2, -270]] #Tourner le moteur 0 90 degrés dans le sens antihoraire, moteur 2180 degrés, moteur 2270 degrés dans le sens antihoraire(=90 degrés dans le sens des aiguilles d'une montre)Tournez-vous vers
Il y a une raison dépendante du matériel pour l'utiliser uniquement dans le sens antihoraire ici. Pour plus de détails, consultez la section sur le matériel.
rot_optimise()
#Optimisation des procédures du robot
def rot_optimise():
global rot
i = 0
tmp_arr = [0, -3, -2, -1]
while i < len(rot):
if i < len(rot) - 1 and rot[i][0] == rot[i + 1][0]:
tmp = tmp_arr[(rot[i][1] + rot[i + 1][1]) % 4]
del rot[i + 1]
if not tmp:
del rot[i]
i -= 1
else:
rot[i][1] = tmp
elif i < len(rot) - 2 and rot[i][0] == rot[i + 2][0] and rot[i][0] % 2 == rot[i + 1][0] % 2:
tmp = tmp_arr[rot[i][1] + rot[i + 2][1] + 2]
del rot[i + 2]
if tmp == 0:
del rot[i]
i -= 1
else:
rot[i][1] = tmp
i += 1
La procédure spécifique au robot générée comprend, par exemple, un tableau de pourriture.
rot = [[0, -90], [0, -90], [2, -90]]
Il peut être simplifié comme suit. Le symbole de rotation dans ce cas est F'F'R '. Cela peut être simplifié en F2 R '.
rot = [[0, -180], [2, -90]]
Nous faisons cette simplification dans cette fonction.
Processus d'inspection
Maintenant, le processus principal (le processus de recherche d'une solution au puzzle).
#Processus d'inspection
def inspection_p():
global ans, rot, colors
ans = []
rot = []
colors = [['' for _ in range(8)] for _ in range(6)]
grab_p()
for i in range(2):
move_actuator(i, 1, 2000)
detect()
strt = time()
#Créer un tableau d'état de puzzle à partir des informations de couleur
confirm_p()
puzzle = Cube()
set_parts_color = [set(i) for i in parts_color]
for i in range(7):
tmp = []
for j in range(3):
tmp.append(colors[parts_place[i][j][0]][parts_place[i][j][1]])
tmp1 = 'w' if 'w' in tmp else 'y'
puzzle.Co[i] = tmp.index(tmp1)
if not set(tmp) in set_parts_color:
solutionvar.set('cannot solve!')
print('cannot solve!')
return
puzzle.Cp[i] = set_parts_color.index(set(tmp))
tmp2 = list(set(range(7)) - set(puzzle.Cp))
if len(tmp2):
tmp2 = tmp2[0]
for i in range(7):
if puzzle.Cp[i] > tmp2:
puzzle.Cp[i] -= 1
print('scramble:')
for i in range(6):
print(colors[i])
print(puzzle.Cp)
print(puzzle.Co)
#Créez un tableau d'états résolus à partir de l'orientation du puzzle
solved_color = [['' for _ in range(8)] for _ in range(6)]
solved_color[5][2] = colors[5][2]
solved_color[3][7] = colors[3][7]
solved_color[3][0] = colors[3][0]
solved_color[2][2] = j2color[(j2color.index(solved_color[3][7]) // 2) * 2 - j2color.index(solved_color[3][7]) % 2 + 1]
solved_color[3][4] = j2color[(j2color.index(solved_color[3][0]) // 2) * 2 - j2color.index(solved_color[3][0]) % 2 + 1]
solved_color[0][2] = j2color[(j2color.index(solved_color[5][2]) // 2) * 2 - j2color.index(solved_color[5][2]) % 2 + 1]
for i in range(6):
for j in range(8):
if (1 < i < 4 or 1 < j < 4) and solved_color[i][j] == '':
if i % 2 and j % 2:
dx = [0, -1, -1]
dy = [-1, -1, 0]
elif i % 2 and (not j % 2):
dx = [0, 1, 1]
dy = [-1, -1, 0]
elif (not i % 2) and j % 2:
dx = [-1, -1, 0]
dy = [0, 1, 1]
elif (not i % 2) and (not j % 2):
dx = [1, 1, 0]
dy = [0, 1, 1]
#print(i, j, dx, dy)
for k in range(3):
if solved_color[i + dy[k]][j + dx[k]] != '':
solved_color[i][j] = solved_color[i + dy[k]][j + dx[k]]
solved = Cube()
for i in range(7):
tmp = []
for j in range(3):
tmp.append(solved_color[parts_place[i][j][0]][parts_place[i][j][1]])
tmp1 = 'w' if 'w' in tmp else 'y'
solved.Co[i] = tmp.index(tmp1)
solved.Cp[i] = set_parts_color.index(set(tmp))
tmp2 = list(set(range(7)) - set(solved.Cp))
if len(tmp2):
tmp2 = tmp2[0]
for i in range(7):
if solved.Cp[i] > tmp2:
solved.Cp[i] -= 1
print('solved:')
for i in range(6):
print(solved_color[i])
print(solved.Cp)
print(solved.Co)
#Séquences Co et cp pour la taille
direction = -1
direction_arr = [21, 12, 15, 18, 2, 22, 20, 4, 8, 13, 23, 1, 6, 0, 3, 9, 11, 16, 14, 7, 5, 19, 17, 10]
for idx, d in enumerate(direction_arr):
if solved_color[5][2] == parts_color[d // 3][d % 3] and solved_color[3][7] == parts_color[d // 3][(d % 3 + 1) % 3]:
direction = idx
if direction == -1:
solutionvar.set('cannot solve!')
print('cannot solve!')
return
with open('cp'+ str(direction) + '.csv', mode='r') as f:
cp = [int(i) for i in f.readline().replace('\n', '').split(',')]
with open('co'+ str(direction) + '.csv', mode='r') as f:
co = [int(i) for i in f.readline().replace('\n', '').split(',')]
print('pre', time() - strt, 's')
#Recherche prioritaire en profondeur avec élagage
def dfs(status, depth, num):
global ans
if num + max(cp[status.cp2i()], co[status.co2i()]) <= depth:
l_mov = ans[-1] if num else -1
t = (l_mov // 3) * 3
lst = set(range(9)) - set([t, t + 1, t + 2])
for mov in lst:
n_status = status.move(mov)
ans.append(mov)
if num + 1 == depth and n_status.Cp == solved.Cp and n_status.Co == solved.Co:
return True
if dfs(n_status, depth, num + 1):
return True
ans.pop()
return False
# IDA*
for depth in range(1, 12):
ans = []
if dfs(puzzle, depth, 0):
break
if ans:
print('answer:', num2moves(ans))
solutionvar.set(num2moves(ans))
rot, _, _ = proc_motor(rot, 0, 4)
print('before:', len(rot))
print(rot)
rot_optimise()
print('after:', len(rot))
print(rot)
print('all', time() - strt, 's')
else:
solutionvar.set('cannot solve!')
print('cannot solve!')
C'est long. Pourquoi n'en avez-vous pas fait une fonction? Je vais expliquer chaque bloc.
Créer un tableau d'état de puzzle à partir des informations de couleur
#Créer un tableau d'état de puzzle à partir des informations de couleur
confirm_p()
puzzle = Cube()
set_parts_color = [set(i) for i in parts_color]
for i in range(7):
tmp = []
for j in range(3):
tmp.append(colors[parts_place[i][j][0]][parts_place[i][j][1]])
tmp1 = 'w' if 'w' in tmp else 'y'
puzzle.Co[i] = tmp.index(tmp1)
if not set(tmp) in set_parts_color:
solutionvar.set('cannot solve!')
print('cannot solve!')
return
puzzle.Cp[i] = set_parts_color.index(set(tmp))
tmp2 = list(set(range(7)) - set(puzzle.Cp))
if len(tmp2):
tmp2 = tmp2[0]
for i in range(7):
if puzzle.Cp[i] > tmp2:
puzzle.Cp[i] -= 1
print('scramble:')
for i in range(6):
print(colors[i])
print(puzzle.Cp)
print(puzzle.Co)
Dans cette partie, le tableau CO (puzzle.Co) et le tableau CP (puzzle.Cp) sont générés à partir des couleurs du tableau de couleurs. Je cherche juste honnêtement, mais la mise en œuvre est un peu lourde. parts_color et parts_place sont dans le tableau suivant.
parts_place = [[[0, 2], [2, 0], [2, 7]], [[0, 3], [2, 6], [2, 5]], [[1, 2], [2, 2], [2, 1]], [[1, 3], [2, 4], [2, 3]], [[4, 2], [3, 1], [3, 2]], [[4, 3], [3, 3], [3, 4]], [[5, 3], [3, 5], [3, 6]], [[5, 2], [3, 7], [3, 0]]]
parts_color = [['w', 'o', 'b'], ['w', 'b', 'r'], ['w', 'g', 'o'], ['w', 'r', 'g'], ['y', 'o', 'g'], ['y', 'g', 'r'], ['y', 'r', 'b'], ['y', 'b', 'o']]
Créez un tableau d'états résolus à partir de l'orientation du puzzle
#Créez un tableau d'états résolus à partir de l'orientation du puzzle
solved_color = [['' for _ in range(8)] for _ in range(6)]
solved_color[5][2] = colors[5][2]
solved_color[3][7] = colors[3][7]
solved_color[3][0] = colors[3][0]
solved_color[2][2] = j2color[(j2color.index(solved_color[3][7]) // 2) * 2 - j2color.index(solved_color[3][7]) % 2 + 1]
solved_color[3][4] = j2color[(j2color.index(solved_color[3][0]) // 2) * 2 - j2color.index(solved_color[3][0]) % 2 + 1]
solved_color[0][2] = j2color[(j2color.index(solved_color[5][2]) // 2) * 2 - j2color.index(solved_color[5][2]) % 2 + 1]
for i in range(6):
for j in range(8):
if (1 < i < 4 or 1 < j < 4) and solved_color[i][j] == '':
if i % 2 and j % 2:
dx = [0, -1, -1]
dy = [-1, -1, 0]
elif i % 2 and (not j % 2):
dx = [0, 1, 1]
dy = [-1, -1, 0]
elif (not i % 2) and j % 2:
dx = [-1, -1, 0]
dy = [0, 1, 1]
elif (not i % 2) and (not j % 2):
dx = [1, 1, 0]
dy = [0, 1, 1]
#print(i, j, dx, dy)
for k in range(3):
if solved_color[i + dy[k]][j + dx[k]] != '':
solved_color[i][j] = solved_color[i + dy[k]][j + dx[k]]
solved = Cube()
for i in range(7):
tmp = []
for j in range(3):
tmp.append(solved_color[parts_place[i][j][0]][parts_place[i][j][1]])
tmp1 = 'w' if 'w' in tmp else 'y'
solved.Co[i] = tmp.index(tmp1)
solved.Cp[i] = set_parts_color.index(set(tmp))
tmp2 = list(set(range(7)) - set(solved.Cp))
if len(tmp2):
tmp2 = tmp2[0]
for i in range(7):
if solved.Cp[i] > tmp2:
solved.Cp[i] -= 1
print('solved:')
for i in range(6):
print(solved_color[i])
print(solved.Cp)
print(solved.Co)
Supposons que l'orientation du puzzle n'est pas une orientation fixe, mais que les couleurs sont saisies au hasard. Pour bien gérer ce cas, créez d'abord un tableau solved_color qui contient l'état de couleur du puzzle à l'état résolu. Où est j2color.
j2color = ['g', 'b', 'r', 'o', 'y', 'w']
Ensuite, nous créerons réellement résolu (classe Cube). Étant donné que l'orientation du puzzle est aléatoire, le tableau résolu ne peut pas être déterminé de manière unique.
Recherche par IDA *
#Séquences Co et cp pour la taille
direction = -1
direction_arr = [21, 12, 15, 18, 2, 22, 20, 4, 8, 13, 23, 1, 6, 0, 3, 9, 11, 16, 14, 7, 5, 19, 17, 10]
for idx, d in enumerate(direction_arr):
if solved_color[5][2] == parts_color[d // 3][d % 3] and solved_color[3][7] == parts_color[d // 3][(d % 3 + 1) % 3]:
direction = idx
if direction == -1:
solutionvar.set('cannot solve!')
print('cannot solve!')
return
with open('cp'+ str(direction) + '.csv', mode='r') as f:
cp = [int(i) for i in f.readline().replace('\n', '').split(',')]
with open('co'+ str(direction) + '.csv', mode='r') as f:
co = [int(i) for i in f.readline().replace('\n', '').split(',')]
print('pre', time() - strt, 's')
#Recherche prioritaire en profondeur avec élagage
def dfs(status, depth, num):
global ans
if num + max(cp[status.cp2i()], co[status.co2i()]) <= depth:
l_mov = ans[-1] if num else -1
t = (l_mov // 3) * 3
lst = set(range(9)) - set([t, t + 1, t + 2])
for mov in lst:
n_status = status.move(mov)
ans.append(mov)
if num + 1 == depth and n_status.Cp == solved.Cp and n_status.Co == solved.Co:
return True
if dfs(n_status, depth, num + 1):
return True
ans.pop()
return False
# IDA*
for depth in range(1, 12):
ans = []
if dfs(puzzle, depth, 0):
break
Voici la recherche qui est finalement sortie dans l'édition de l'algorithme. Cependant, il est très différent du programme de version d'algorithme. Dans la section algorithme, la recherche de priorité de profondeur a été écrite à l'aide de pile au lieu de récursive car elle met l'accent sur la lisibilité, mais ici, récursive est utilisée pour réduire avec succès l'utilisation de la mémoire à la limite maximale. De plus, comme il n'y a que 24 variations de séquences d'élagage (2 ensembles de CP et CO), le programme suivant pré-calcule et crée un fichier csv.
import csv
from collections import deque
class Cube:
def __init__(self):
self.Co = [0, 0, 0, 0, 0, 0, 0]
self.Cp = [0, 1, 2, 3, 4, 5, 6]
self.Moves = []
#self.Movnum = 0
#Traitement de rotation CP
def move_cp(self, num):
surface = [[0, 1, 2, 3], [2, 3, 4, 5], [3, 1, 5, 6]]
replace = [[2, 0, 3, 1], [3, 2, 1, 0], [1, 3, 0, 2]]
idx = num // 3
res = Cube()
res.Cp = [i for i in self.Cp]
for i, j in zip(surface[idx], replace[num % 3]):
res.Cp[i] = self.Cp[surface[idx][j]]
res.Moves = [i for i in self.Moves]
res.Moves.append(num)
return res
#Traitement de rotation CO
def move_co(self, num):
surface = [[0, 1, 2, 3], [2, 3, 4, 5], [3, 1, 5, 6]]
replace = [[2, 0, 3, 1], [3, 2, 1, 0], [1, 3, 0, 2]]
pls = [2, 1, 1, 2]
idx = num // 3
res = Cube()
res.Co = [i for i in self.Co]
for i, j in zip(surface[idx], replace[num % 3]):
res.Co[i] = self.Co[surface[idx][j]]
if num // 3 != 0 and num % 3 != 1:
for i in range(4):
res.Co[surface[idx][i]] += pls[i]
res.Co[surface[idx][i]] %= 3
res.Moves = [i for i in self.Moves]
res.Moves.append(num)
return res
#Changez en fait la disposition des états du puzzle en fonction du numéro de rotation
def move(self, num):
res = Cube()
res = self.move_co(num)
res.Cp = self.move_cp(num).Cp
return res
#Créer un numéro unique à partir du tableau cp
def cp2i(self):
res = 0
marked = set([])
for i in range(7):
res += fac[6 - i] * len(set(range(self.Cp[i])) - marked)
marked.add(self.Cp[i])
return res
#Créer un numéro unique à partir du tableau co
def co2i(self):
res = 0
for i in self.Co:
res *= 3
res += i
return res
parts_place = [[[0, 2], [2, 0], [2, 7]], [[0, 3], [2, 6], [2, 5]], [[1, 2], [2, 2], [2, 1]], [[1, 3], [2, 4], [2, 3]], [[4, 2], [3, 1], [3, 2]], [[4, 3], [3, 3], [3, 4]], [[5, 3], [3, 5], [3, 6]], [[5, 2], [3, 7], [3, 0]]]
parts_color = [['w', 'o', 'b'], ['w', 'b', 'r'], ['w', 'g', 'o'], ['w', 'r', 'g'], ['y', 'o', 'g'], ['y', 'g', 'r'], ['y', 'r', 'b'], ['y', 'b', 'o']]
j2color = ['g', 'b', 'r', 'o', 'y', 'w']
direction_arr = [21, 12, 15, 18, 2, 22, 20, 4, 8, 13, 23, 1, 6, 0, 3, 9, 11, 16, 14, 7, 5, 19, 17, 10]
fac = [1]
for i in range(1, 8):
fac.append(fac[-1] * i)
for idx, d in enumerate(direction_arr):
set_parts_color = [set(i) for i in parts_color]
solved_color = [['' for _ in range(8)] for _ in range(6)]
solved_color[5][2] = parts_color[d // 3][d % 3]
solved_color[3][7] = parts_color[d // 3][(d % 3 + 1) % 3]
solved_color[3][0] = parts_color[d // 3][(d % 3 + 2) % 3]
solved_color[2][2] = j2color[(j2color.index(solved_color[3][7]) // 2) * 2 - j2color.index(solved_color[3][7]) % 2 + 1]
solved_color[3][4] = j2color[(j2color.index(solved_color[3][0]) // 2) * 2 - j2color.index(solved_color[3][0]) % 2 + 1]
solved_color[0][2] = j2color[(j2color.index(solved_color[5][2]) // 2) * 2 - j2color.index(solved_color[5][2]) % 2 + 1]
for i in range(6):
for j in range(8):
if (1 < i < 4 or 1 < j < 4) and solved_color[i][j] == '':
if i % 2 and j % 2:
dx = [0, -1, -1]
dy = [-1, -1, 0]
elif i % 2 and (not j % 2):
dx = [0, 1, 1]
dy = [-1, -1, 0]
elif (not i % 2) and j % 2:
dx = [-1, -1, 0]
dy = [0, 1, 1]
elif (not i % 2) and (not j % 2):
dx = [1, 1, 0]
dy = [0, 1, 1]
#print(i, j, dx, dy)
for k in range(3):
if solved_color[i + dy[k]][j + dx[k]] != '':
solved_color[i][j] = solved_color[i + dy[k]][j + dx[k]]
solved = Cube()
for i in range(7):
tmp = []
for j in range(3):
tmp.append(solved_color[parts_place[i][j][0]][parts_place[i][j][1]])
tmp1 = 'w' if 'w' in tmp else 'y'
solved.Co[i] = tmp.index(tmp1)
solved.Cp[i] = set_parts_color.index(set(tmp))
tmp2 = list(set(range(7)) - set(solved.Cp))
if len(tmp2):
tmp2 = tmp2[0]
for i in range(7):
if solved.Cp[i] > tmp2:
solved.Cp[i] -= 1
print('solved:')
for i in range(6):
print(solved_color[i])
print(solved.Cp)
print(solved.Co)
print(idx)
#Séquences Co et cp pour la taille
inf = 100
cp = [inf for _ in range(fac[7])]
cp_solved = Cube()
cp_solved.Cp = solved.Cp
cp[cp_solved.cp2i()] = 0
que = deque([cp_solved])
while len(que):
status = que.popleft()
num = len(status.Moves)
l_mov = status.Moves[-1] if num else -1
t = (l_mov // 3) * 3
lst = set(range(9)) - set([t, t + 1, t + 2])
for mov in lst:
n_status = status.move_cp(mov)
n_idx = n_status.cp2i()
if cp[n_idx] == inf:
cp[n_idx] = len(n_status.Moves) #n_status.Movnum
que.append(n_status)
co = [inf for _ in range(3 ** 7)]
co_solved = Cube()
co_solved.Co = solved.Co
co[co_solved.co2i()] = 0
que = deque([co_solved])
while len(que):
status = que.popleft()
num = len(status.Moves)
l_mov = status.Moves[-1] if num else -1
t = (l_mov // 3) * 3
lst = set(range(9)) - set([t, t + 1, t + 2])
for mov in lst:
n_status = status.move_co(mov)
n_idx = n_status.co2i()
if co[n_idx] == inf:
co[n_idx] = len(n_status.Moves) #n_status.Movnum
que.append(n_status)
with open('cp' + str(idx) + '.csv', mode='x') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(cp)
with open('co' + str(idx) + '.csv', mode='x') as f:
writer = csv.writer(f, lineterminator='\n')
writer.writerow(co)
Le cours étant un peu différent de celui expliqué précédemment, le programme à publier est devenu long, mais ce n'est pas un gros problème. Honnêtement, le nombre le plus court d'étapes requises pour aligner CO ou CP pour 24 modes de détention est résumé dans csv.
Générer un tableau pour déplacer le robot
if ans:
print('answer:', num2moves(ans))
solutionvar.set(num2moves(ans))
rot, _, _ = proc_motor(rot, 0, 4)
print('before:', len(rot))
print(rot)
rot_optimise()
print('after:', len(rot))
print(rot)
print('all', time() - strt, 's')
else:
solutionvar.set('cannot solve!')
print('cannot solve!')
Ici, la réponse en tant que symbole de rotation lu par les humains est convertie en un tableau pour déplacer le robot. Il affiche également un symbole de rotation à l'écran. Si les cubes ne sont pas alignés, impossible de colve! S'affiche.
En fait, déplacez le robot
Ici, le robot est réellement déplacé en communiquant avec l'ATMEGA328P.
#En fait, déplacez le robot
def start_p():
print('start!')
strt_solv = time()
i = 0
while i < len(rot):
if GPIO.input(4) == GPIO.LOW:
solvingtimevar.set('emergency stop')
print('emergency stop')
return
grab = rot[i][0] % 2
for j in range(2):
move_actuator(j, grab, 1000)
sleep(0.4)
for j in range(2):
move_actuator(j, (grab + 1) % 2, 2000)
sleep(0.1)
ser_num = rot[i][0] // 2
rpm = 100
offset = -5
move_actuator(ser_num, rot[i][0] % 2, rot[i][1] * 90 + offset, rpm)
max_turn = abs(rot[i][1])
flag = i < len(rot) - 1 and rot[i + 1][0] % 2 == rot[i][0] % 2
if flag:
move_actuator(rot[i + 1][0] // 2, rot[i + 1][0] % 2, rot[i + 1][1] * 90 + offset, rpm)
max_turn = max(max_turn, abs(rot[i + 1][1]))
slptim = 60 / rpm * (max_turn * 90 + offset) / 360 * 1.1
sleep(slptim)
move_actuator(ser_num, rot[i][0] % 2, -offset, rpm)
if flag:
move_actuator(rot[i + 1][0] // 2, rot[i + 1][0] % 2, -offset, rpm)
i += 1
i += 1
slptim2 = abs(60 / rpm * offset / 360) * 1.1
sleep(slptim2)
print('done', i, 'sleep:', slptim, slptim2)
solv_time = time() - strt_solv
solvingtimevar.set(str(round(solv_time, 3)) + 's')
print('solving time:', solv_time, 's')
Les moteurs 0 et 2 ou 1 et 3 correspondent respectivement aux moteurs U / B et R / L, de sorte qu'ils peuvent être tournés en même temps. Dans un tel cas, nous essayons d'accélérer en tournant en même temps.
De plus, lorsque je fais tourner le moteur, j'essaye d'attendre le temps qu'il tourne (1,1 fois).
ATMEGA328P - C++ Le contenu de Python était assez lourd. Puisque le programme ATMEGA328P est simple, je vais tout montrer en même temps sans le diviser.
#include <Servo.h>
const long turn_steps = 400;
const int step_dir[2] = {3, 7};
const int step_pul[2] = {4, 8};
char buf[30];
int idx = 0;
long data[3];
Servo servo0;
Servo servo1;
void move_motor(long num, long deg, long spd) {
bool hl = true;
if (deg < 0) hl = false;
digitalWrite(step_dir[num], hl);
long wait_time = 1000000 * 60 / turn_steps / spd;
long steps = abs(deg) * turn_steps / 360;
bool motor_hl = false;
for (int i = 0; i < steps; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(wait_time);
}
}
void release_arm(int num) {
if (num == 0)servo0.write(120);
else servo1.write(120);
}
void grab_arm(int num) {
if (num == 0)servo0.write(60);
else servo1.write(60);
}
void setup() {
Serial.begin(9600);
for (int i = 0; i < 2; i++) {
pinMode(step_dir[i], OUTPUT);
pinMode(step_pul[i], OUTPUT);
}
servo0.attach(5);
servo1.attach(6);
}
void loop() {
if (Serial.available()) {
buf[idx] = Serial.read();
if (buf[idx] == '\n') {
buf[idx] = '\0';
data[0] = atoi(strtok(buf, " "));
data[1] = atoi(strtok(NULL, " "));
data[2] = atoi(strtok(NULL, " "));
if (data[1] == 1000) grab_arm(data[0]);
else if (data[1] == 2000) release_arm(data[0]);
else move_motor(data[0], data[1], data[2]);
idx = 0;
}
else {
idx++;
}
}
}
Je présenterai chaque fonction.
turning_time(int deg, int speed_motor)
float turning_time(int deg, int speed_motor) {
return abs(1000 * quarter * deg / turn_steps * 60 / speed_motor);
}
Cette fonction renvoie le temps (en millisecondes) nécessaire pour déplacer le moteur en saisissant l'angle et la vitesse de déplacement du moteur.
move_motor(int num, int deg, int spd)
void move_motor(long num, long deg, long spd) {
bool hl = true;
if (deg < 0) hl = false;
digitalWrite(step_dir[num], hl);
long wait_time = 1000000 * 60 / turn_steps / spd;
long steps = abs(deg) * turn_steps / 360;
bool motor_hl = false;
for (int i = 0; i < steps; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(wait_time);
}
}
C'est une fonction qui déplace le moteur pas à pas. J'ai utilisé le A4988 comme pilote de moteur (environ 1000 yens pour 5 chez amazon), donc le style d'écriture y est adapté.
Lorsque step_dir [num] est réglé sur HIGH, c'est une rotation avant, et quand il est LOW, c'est une rotation inverse. Et chaque fois que vous envoyez une impulsion à step_pul [num], elle se déplace pas à pas. Pour plus de détails sur l'utilisation du A4988, voir ici.
release_arm(int num)
void release_arm(int num) {
if (num == 0)servo0.write(120);
else servo1.write(120);
}
Une fonction qui libère le cube avec le bras. Réglez l'angle d'asservissement sur 120 degrés.
grab_arm(int num)
void grab_arm(int num) {
if (num == 0)servo0.write(60);
else servo1.write(60);
}
C'est une fonction pour saisir le cube avec le bras. Réglez l'angle du servo sur 60 degrés.
loop()
void loop() {
if (Serial.available()) {
buf[idx] = Serial.read();
if (buf[idx] == '\n') {
buf[idx] = '\0';
data[0] = atoi(strtok(buf, " "));
data[1] = atoi(strtok(NULL, " "));
data[2] = atoi(strtok(NULL, " "));
if (data[1] == 1000) grab_arm(data[0]);
else if (data[1] == 2000) release_arm(data[0]);
else move_motor(data[0], data[1], data[2]);
idx = 0;
}
else {
idx++;
}
}
}
Divisez la commande envoyée, séparée par des espaces, et exécutez move_motor, grab_arm ou release_arm en l'utilisant comme requête.
Résumé
Merci beaucoup d'avoir lu jusqu'ici! C'est un programme d'environ 800 lignes au total, et c'est difficile à introduire et à lire ... Cette fois, je n'ai introduit que les fonctions des fonctions et je n'ai pas touché aux détails. De plus, si vous n'avez pas encore lu l'édition du matériel, vous ne comprenez peut-être pas le mécanisme autour du bras. Si vous avez des questions, laissez un commentaire.
Recommended Posts