Let's make a robot that solves the Rubik's Cube! 3 Software
What is this article?
This article is serialized ** Let's make a robot that solves the Rubik's cube! It is a part of the article collection called **. Click here for the whole picture
GitHub is here.
The promotional video is here.
The video collection about this article collection is here.
Contents of this article
In this article, I will explain the software of Soltvvo, a robot that solves the 2x2x2 Rubik's cube I made.
Constitution
Before explaining the software, it is necessary to write down the configuration of the robot. It looks like the figure.
Here, the Raspberry Pi is a single board computer called Raspberry Pi 4 (2GB RAM) (that is, an ordinary (?) Computer), and the ATMEGA328P is a famous microcomputer (microcontroller), which is used for the Arduino microcomputer board. This time I will use it as Arduino Uno. There are two stepping motors and two servo motors from ATMEGA328P. This is the actuator (power) that actually moves the arm that turns the cube. You can also use the webcam to enter the state of the cube.
The whole picture
I will show you the whole picture of the program.
Program outline on the Raspberry Pi side
- A class that represents the state of the cube
- Easy management of CP and CO status
- Easy description to operate the cube
- Function to move the actuator
- Send a command to move the arm
- GUI
- Screen display
- Get the puzzle status using the camera
- Image recognition
- Search for a solution
- IDA * Search
- Pre-calculation is calculated in advance and made into a csv file
- Make a procedure for the robot to move from the list of rotation symbols
- Try to get the best solution
- Actually move the robot
Program outline on the ATMEGA328P side
- Receive command
- Function to move the motor
- Function to grab the cube with the arm
- Function to release the cube with the arm
Raspberry Pi side program-Python
Here, we will explain the program on the PC side. Since the program has about 580 lines and is long, I will explain it by dividing it into several parts.
Library import
Import the library you want to use.
from copy import deepcopy
from collections import deque
from time import time, sleep
import tkinter
import cv2
import numpy as np
import serial
Cube class
I created a Cube class because it is good to think of a cube as one "thing" in the program. I am ashamed to say that this is my first class in my life.
class Cube:
def __init__(self):
self.Co = [0, 0, 0, 0, 0, 0, 0]
self.Cp = [0, 1, 2, 3, 4, 5, 6]
#Rotation processing 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
#Rotation processing 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
#Actually change the state array of the puzzle according to the rotation number
def move(self, num):
res = Cube()
res.Co = self.move_co(num)
res.Cp = self.move_cp(num)
return res
#Create a unique number from the cp array
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
#Create a unique number from the co array
def co2i(self):
res = 0
for i in self.Co:
res *= 3
res += i
return res
I will explain the functions one by one. __init__(self)
def __init__(self):
self.Co = [0, 0, 0, 0, 0, 0, 0]
self.Cp = [0, 1, 2, 3, 4, 5, 6]
This is the first function to be executed when calling the Cube class. Here, we associate two arrays, Co and Cp, with the Cube class. These are, in order, CO (abbreviation for Corner Orientation, which indicates the orientation of corner parts) and CP (abbreviation for Corner Permutation, which indicates the position of corner parts).
In addition, it is very efficient to have your own CO sequence and CP sequence to maintain the state of the cube. Also, since there are only R, U, and F to turn, the DLB (lower left back) parts do not move. You don't need to keep the information for this part, you only need 7 data for each CO and CP.
CP numbers the parts as shown and holds the values so that the part at the index location of the element in the CP array is the element in the CP array.
The CO is 0 when the white or yellow sticker appears on the U or D side, and becomes 1 or 2 each time it is rotated 120 degrees clockwise from there. As an example, give a CO number to the photo.
move_cp(self, num)
#Rotation processing 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
This function is (a part of) the function that actually rotates the cube. Rotate only the CP of the array. I don't mess with CO. In addition, num represents the number of the rotation symbol (rotation number), and corresponds to the index of the following array placed globally.
move_candidate = ["U", "U2", "U'", "F", "F2", "F'", "R", "R2", "R'"] #Candidates for rotation
The surface array shows the parts that move when the U, F, and R planes are moved, and the replace array shows how the parts move when X, X2, and X'are moved, respectively. Rotation processing is made possible by successfully substituting these arrays.
move_co(self, num)
#Rotation processing 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
This function, like the move_cp function above, is a function that actually moves the part. This function drives CO. The content is very similar to the move_cp function, but the CO changes when the U plane is not rotated (num% 3! = 0) and when it is not rotated 180 degrees (num% 3! = 1), so it needs to be processed appropriately. there is.
move(self, num)
#Actually change the state array of the puzzle according to the rotation number
def move(self, num):
res = Cube()
res.Co = self.move_co(num)
res.Cp = self.move_cp(num)
return res
This function is also a function that actually moves the part. This function runs both CO and CP. I'm just running the two functions above.
cp2i(self)
#Create a unique number from the cp array
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
This function generates and returns a CP array-specific number from the CP array. It just returns the permutation numbers of the CP array. What is a permutation number? 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 It is the number when arranged in the order like. I think the site of here is easy to understand how to calculate this number.
co2i(self)
#Create a unique number from the co array
def co2i(self):
res = 0
for i in self.Co:
res *= 3
res += i
return res
This function generates and returns a CO array-specific number from the CO array. At this time, a unique number is created by calculating the CO array as a 7-digit ternary number.
Functions related to other puzzles
There is a function called num2moves that translates rotation numbers into rotation symbols.
#Convert rotation number to rotation symbol
def num2moves(arr):
res = ''
for i in arr:
res += move_candidate[i] + ' '
return res
move_candidate is
move_candidate = ["U", "U2", "U'", "F", "F2", "F'", "R", "R2", "R'"] #Candidates for rotation
is.
Function to move the actuator
The whole picture is here.
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)
#Send a command to move the actuator
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)
This function sends a command to ATMEGA328P by serial communication to move the motor. Here, num is the ATMEGA328P number (0 or 1), arg1 is the actuator number, arg2 is the amount to operate the actuator, and arg3 is the motor speed (rpm) when moving the stepping motor. ..
Regarding serial communication,
ser_motor[0] = serial.Serial('/dev/ttyUSB0', 9600, write_timeout=0)
ser_motor[1] = serial.Serial('/dev/ttyUSB1', 9600, write_timeout=0)
There is a definition. Each ATMEGA328P has two stepper motors and two servo motors.
grab_p()
#Grab the cube
def grab_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 1000)
This function is a function that uses the move_actuator function mentioned earlier to move all the arms at once and grab the cube.
release_p()
#Release the cube
def release_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 2000)
This function moves all the arms at once and releases the cube.
GUI The part related to GUI is here.
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()
I use tkinter for the GUI library. I will explain what each variable corresponds to in the figure.
Although not shown here, there is a label called solving time that shows on the screen how long it took to solve the cube.
The reason why it is the entry box that displays the color is that this program originally entered the state of the cube by typing the color in the entry box. Such an era is over and now it is recognized by the camera.
Store puzzle colors in an array
The whole picture is here.
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']]
#Reflect the color in the box
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]]
#Fill the place where the color is fixed where it is not filled
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
#Change the background color of the unfilled area to gray
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'
This function is a bit complicated. In the case of 2x2x2, it is enough to look at only the four (well-chosen) sides of the puzzle (for example, the D, F, U, B sides in this program) to uniquely determine the state of the puzzle. Therefore, it is this function that guesses and determines the color of the surface that is not seen. The decided color will be reflected as the background color of the entry box.
This function simply does a full search to find out what the unfilled color will be. However, the implementation has become a little heavy. I'm not confident that this is the best way to write it.
Get the puzzle state from the camera
The whole picture is here.
j2color = ['g', 'b', 'r', 'o', 'y', 'w']
idx = 0
colors = [['' for _ in range(8)] for _ in range(6)]
#Get the state of the 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: #When the space key is pressed
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()
This function gets the color of the puzzle depending on what the color of each of the four pixels in the image taken by the camera is. The screen looks like this.
Create a robot-specific procedure
In the inspection function described later, what is output using IDA * is a list of rotation symbols (rotation numbers) for human reading. Convert this into an array with information about which motors to run, when and how many times. The whole picture is here.
#Determine the motor to turn from the array of rotation symbol numbers
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
#Robot procedure optimization
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
It sounds like you're doing something very stupid. There are a number of ridiculously long hand-made arrays. Let's explain the functions one by one.
proc_motor(rot, num, direction)
#Determine the motor to turn from the array of rotation symbol numbers
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
I will talk about the details in the hardware section, but the robot has four arms, which are attached to the F, R, B, and L sides, respectively. In other words, the U side cannot be turned unless you change hands. Here, switching is like turning the R side and the L side at the same time, for example. There are $ 4 (streets / faces) \ times6 (faces) = 24 ways of holding the cube, so assign a number to each direction and how to switch from each direction to what direction it will be. In the array regrip_arr (this is hard work). There are two ways to switch between RL and FB at the same time (clockwise and counterclockwise are the same value after all).
Based on the current orientation of the cube, we will search for whether the next step can be turned without changing hands, and if it is necessary to change hands, which of the two ways of changing hands is more efficient by a full search using recursion.
Values are stored in the rot array as shown below.
rot = [[Number of motor to turn,Direction and amount of turning], [0, -90], [1, -180], [2, -270]] #Turn motor 0 90 degrees counterclockwise, motor 2 180 degrees, motor 2 270 degrees counterclockwise(=90 degree clockwise)Turn to
There is a hardware-dependent reason to use it only counterclockwise here. For details, see the hardware section.
rot_optimise()
#Robot procedure optimization
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
The generated robot-specific procedure includes, for example, a rot array.
rot = [[0, -90], [0, -90], [2, -90]]
It may be simplified as follows. The rotation symbol in this case is F'F'R'. This can be simplified to F2 R'.
rot = [[0, -180], [2, -90]]
We do this simplification within this function.
Inspection process
Now, the main process (the process of searching for a solution to the puzzle) is finally here.
#Inspection process
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()
#Create a puzzle state array from color information
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)
#Create an array of solved states from the orientation of the 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)
#Co and cp sequences for pruning
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')
#Depth-first search with pruning
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!')
It's long. Why didn't you make it a function? I will explain each block.
Create a puzzle state array from color information
#Create a puzzle state array from color information
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)
In this part, CO array (puzzle.Co) and CP array (puzzle.Cp) are generated from the color array colors. I'm just searching honestly, but the implementation is a little heavy. parts_color and parts_place are the following array.
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']]
Create an array of solved states from the orientation of the puzzle
#Create an array of solved states from the orientation of the 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)
Suppose that the direction of the puzzle is not a fixed direction but a random color is entered. To handle that case well, first create a solved_color array that contains the color state of the puzzle in the solved state. Where j2color is this.
j2color = ['g', 'b', 'r', 'o', 'y', 'w']
Then, we will actually create solved (Cube class). Since the orientation of the puzzle is random, the solved array cannot be uniquely determined.
Search by IDA *
#Co and cp sequences for pruning
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')
#Depth-first search with pruning
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
Here is the search that finally came out in the algorithm edition. However, it is very different from the algorithm version program. In the algorithm section, I wrote a depth-first search using stack instead of recursion to emphasize readability, but here I use recursion to successfully reduce the memory usage to the limit. Also, since there are only 24 variations of the pruning array (2 sets of CP and CO), it is pre-calculated with the following program and made into a csv file.
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
#Rotation processing 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
#Rotation processing 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
#Actually change the state array of the puzzle according to the rotation number
def move(self, num):
res = Cube()
res = self.move_co(num)
res.Cp = self.move_cp(num).Cp
return res
#Create a unique number from the cp array
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
#Create a unique number from the co array
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)
#Co and cp sequences for pruning
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)
Since the class is a little different from the one explained before, the program to be posted has become long, but it is not a big deal. To be honest, the shortest number of steps required to align either CO or CP for 24 ways of holding is summarized in csv.
Generate an array to move the 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!')
Here, the answer as a rotation symbol read by humans is converted into an array for moving the robot. It also displays a rotation symbol on the screen. If the cubes are not aligned, cannot colve! Is displayed.
Actually move the robot
Here, the robot is actually moved by communicating with the ATMEGA328P.
#Actually move the 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')
Motors 0 and 2 or 1 and 3 correspond to U / B and R / L motors, respectively, so they can be turned at the same time. In such a case, we are trying to speed up by turning at the same time.
Also, when I turn the motor, I try to wait for the time it is turning (1.1 times).
ATMEGA328P - C++ The content of Python was quite heavy. Since the program of ATMEGA328P is simple, I will show it all at once without dividing it.
#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++;
}
}
}
I will introduce each function.
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);
}
This function returns the time (in milliseconds) it takes to move the motor by inputting the angle and speed of movement of the motor.
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);
}
}
A function that drives a stepping motor. I used an A4988 motor driver (about 1000 yen for 5 on Amazon), so the writing style is adapted to that.
When step_dir [num] is set to HIGH, it is forward rotation, and when it is LOW, it is reverse rotation. And every time you send a pulse to step_pul [num], it moves one step at a time. For details on how to use A4988, see here.
release_arm(int num)
void release_arm(int num) {
if (num == 0)servo0.write(120);
else servo1.write(120);
}
A function that releases the cube with the arm. Set the servo angle to 120 degrees.
grab_arm(int num)
void grab_arm(int num) {
if (num == 0)servo0.write(60);
else servo1.write(60);
}
It is a function to grab the cube with the arm. Set the servo angle to 60 degrees.
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++;
}
}
}
Split the space-separated command sent and execute move_motor, grab_arm, or release_arm using it as a query.
Summary
Thank you so much for reading this far! It's a program of about 800 lines in total, and it's hard to introduce and read ... This time, I've only introduced the functions of functions and didn't touch on the details. Also, if you haven't read the hardware edition yet, you may not understand the mechanism around the arm. If you have any questions, please leave a comment.
Recommended Posts