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

  1. Overview
  2. Algorithm
  3. Software (this article)
  4. Hardware

GitHub is here.

The promotional video is here. Soltvvo

The video collection about this article collection is here. Let's make a robot that solves the Rubik's Cube!

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. 構成.png

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

Program outline on the ATMEGA328P side

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. パーツ番号.png

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. CO.png

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. gui.png 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. カメラ.PNG

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

Let's make a robot that solves the Rubik's Cube! 3 Software
Let's make a robot that solves the Rubik's Cube! 2 Algorithm
Let's make a robot that solves the Rubik's Cube! 1 Overview
Let's make a remote rumba [Software]
Let's write a program to solve the 4x4x4 Rubik's Cube! 2. Algorithm
Let's write a program to solve the 4x4x4 Rubik's Cube! 3. Implementation
Rubik's Cube Robot Software Updated 7. Key Operations
Updated software for Rubik's Cube Robot 2. Pre-calculation
Updated Rubik's Cube Robot software 3. Solution search
Rubik's Cube Robot Software Updated 1. Basic Functions
Updated Rubik's Cube Robot software 6. Machine operation (Arduino)
Rubik's Cube Robot Software Updated 5. Machine Operation (Python)
Let's make a robot that solves the Rubik's Cube! 3 Software
Let's make a robot that solves the Rubik's Cube! 2 Algorithm
Let's make a robot that solves the Rubik's Cube! 1 Overview
Rubik's Cube Robot Software Updated 7. Key Operations
Updated software for Rubik's Cube Robot 2. Pre-calculation
Updated Rubik's Cube Robot software 3. Solution search
Rubik's Cube Robot Software Updated 1. Basic Functions
Let's write a program to solve the Rubik's Cube (Part 2: IDA * Search)
Let's replace UWSC with Python (5) Let's make a Robot
Updated Rubik's Cube Robot software 6. Machine operation (Arduino)
Rubik's Cube Robot Software Updated 5. Machine Operation (Python)
Make a BOT that shortens the URL of Discord
Let's make a Discord Bot.
Let's make the analysis of the Titanic sinking data like that
Write a program to solve the 4x4x4 Rubik's Cube! 1. Overview
A memo that solves the knapsack problem by the greedy algorithm
Let's make a rock-paper-scissors game
Let's make a diagram that can be clicked with IPython
I made a program that solves the spot the difference in seconds
Let's make a remote rumba [Hardware]
Let's make a GUI with python.
Let's make a spot sale service 2
Let's make a breakout with wxPython
Let's make a spot sale service 1
[Python] Make the function a lambda function
Let's make a graph with python! !!
Let's make a supercomputer with xCAT
Let's make a spot sale service 3
Let's make a shiritori game with Python
A class that hits the DMM API
Let's make a voice slowly with Python
Let's make a simple language with PLY 1
Let's make a multilingual site using flask-babel
Let's make a web framework with Python! (1)
Let's make a tic-tac-toe AI with Pylearn 2
Let's make a combination calculation in Python
A code that corrects the yoon / sokuon (sokuon)
Let's make a Twitter Bot with Python!
[Python] A program that rounds the score
Let's make a web framework with Python! (2)
Let's make a Backend plugin for Errbot
Let's do clustering that gives a nice bird's-eye view of the text dataset
Creating software that mirrors the Android screen to a PC 2 Real-time touch edition
Let's display a simple template that is ideal for Django for the first time
How to make a Raspberry Pi that speaks the tweets of the specified user
How to make a program to solve Rubik's Cube starting from PC Koshien 2014 Floppy Cube