Rubik’s Cube
Documentations > Projets
Posté le 15 novembre 2018 dans Projets par Julien.
Réalisation d'un robot capable de résoudre des Rubik's Cubes 🤖
Description
Objectif : RĂ©aliser un robot capable de rĂ©soudre un Rubik’s Cube.
Technologies utilisées :  | Python 3.6.5  | OpenCV 3.4.1  | Kociemba python
Pré-requis
# - - - - Sous MacOS ->
# Avoir installé homebrew
# Avoir installé python3
brew install python
# - - - - /macos
# Installer OpenCV :
https://juju-dev.fr/applications/opencv/
# Installer Kociemba (algorythme de résolution) :
pip install kociemba
# OU pip3 install kociemba
Master Code (MacOS ou R.Pi)
# - - - - - - 18 mai 2018 - - - - - -
# Imports
import cv2 as cv
import numpy as np
import kociemba
import time
#import serial
import sys
import string
##############################################################################
################################## SETTINGS ##################################
##############################################################################
offset_x = 50; # Position offset X of the cube
offset_y = 50; # Position offset Y of the cube
rk_w = 600 # set rubbikscube width
rk_h = 600 # set rubbikscube height
detec_threshold = 6000 # number of white pixels after mask
# - - - - - - - - - - - - - - - - - Range of colors in HSV :
rl1 = np.array([0,175,25]); ru1 = np.array([5,255,255]) # RED 1
rl2 = np.array([170,175,25]); ru2 = np.array([180,255,255]) # RED 2
gl = np.array([32,38,25]); gu = np.array([75,255,255]) # GREEN
bl = np.array([110,50,50]); bu = np.array([130,255,255]) # BLUE
wl = np.array([0,0,100]); wu = np.array([255,38,255]) # WHITE ( = LIGHT GREY )
yl = np.array([20,50,30]); yu = np.array([35,255,255]) # YELLOW
ol = np.array([5,70,25]); ou = np.array([17,175,255]) # ORANGE
# - - - - - - - - - - - - - - - - -
c_colors = [0,0,0,0,0,0,0,0,0] # Rubiks Cube Current Colors
#############
# 1 2 3 #
# 4 5 6 # FACE VISIBLE
# 7 8 9 #
#############
########## DEBUG -> ###########
# FRONT
face_f = [
"face_color", # facette in center
"red", # 1
"green", # 2
"red", # 3
"white", # 4
"red", # 5 = center = idem face color
"orange", # 6
"yellow", # 7
"yellow", # 8
"yellow" # 9
]
face_f[0] = face_f[5]
# DOWN
face_d = [ "face_color", "blue", "blue", "blue", "red", "white", "red", "yellow", "red", "orange" ]
face_d[0] = face_d[5]
# BACK
face_b = [ "face_color", "orange", "orange", "white", "green", "orange", "yellow", "white", "yellow", "orange" ]
face_b[0] = face_b[5]
# UP
face_u = [ "face_color", "blue", "blue", "green", "white", "yellow", "orange", "green", "red", "green" ]
face_u[0] = face_u[5]
# LEFT
face_l = [ "face_color", "red", "blue", "yellow", "orange", "blue", "green", "green", "white","orange" ]
face_l[0] = face_l[5]
# RIGHT
face_r = [ "face_color", "white", "green", "white", "white", "green", "yellow", "red", "blue", "blue" ]
face_r[0] = face_r[5]
########## /debug ###########
##############################################################################
################################# /settings ##################################
##############################################################################
##############################################################################
################################# FUNCTIONS ##################################
##############################################################################
# CROP INPUT STREAM IN 9 IMAGES
def crop(input): # input is "frame"
# Init
x=0; y=0; # Default position
# Crop stettings - - - - - -
# Size :
w = int(rk_w/3);
h = int(rk_h/3);
# Offset :
o_x = offset_x;
o_y = offset_y;
# - - - - - - - - - - - - - -
# One crop for each kubbiks cube color on 1 face (See above in settings what is 1,2,3,4,...)
x=o_x+(0*w); y=o_y+(0*h); crop_img_1=input[y:y+h,x:x+w]; ### 1
x=o_x+(1*w); y=o_y+(0*h); crop_img_2=input[y:y+h,x:x+w]; ### 2
x=o_x+(2*w); y=o_y+(0*h); crop_img_3=input[y:y+h,x:x+w]; ### 3
x=o_x+(0*w); y=o_y+(1*h); crop_img_4=input[y:y+h,x:x+w]; ### 4
x=o_x+(1*w); y=o_y+(1*h); crop_img_5=input[y:y+h,x:x+w]; ### 5
x=o_x+(2*w); y=o_y+(1*h); crop_img_6=input[y:y+h,x:x+w]; ### 6
x=x=o_x+(0*w); y=o_y+(2*h); crop_img_7=input[y:y+h,x:x+w]; ### 7
x=x=o_x+(1*w); y=o_y+(2*h); crop_img_8=input[y:y+h,x:x+w]; ### 8
x=x=o_x+(2*w); y=o_y+(2*h); crop_img_9=input[y:y+h,x:x+w]; ### 9
# Return cropped images tab
tab_cropped_img = [ crop_img_1, crop_img_2, crop_img_3, crop_img_4, crop_img_5, crop_img_6, crop_img_7, crop_img_8, crop_img_9 ]
return tab_cropped_img
# FIND COLOR WE WANT | range = cl<->cu | cl=color_low, cu=color_up
def findColor( input,name, f_pos_x,f_pos_y, ) :
# Convert BGR to HSV
hsv = cv.cvtColor(input, cv.COLOR_BGR2HSV)
# Create a black and white mask for each colors
mask_red1 = cv.inRange( hsv, rl1, ru1 )
mask_red2 = cv.inRange( hsv, rl1, ru2 )
mask_green = cv.inRange( hsv, gl, gu )
mask_blue = cv.inRange( hsv, bl, bu )
mask_white = cv.inRange( hsv, wl, wu )
mask_yellow = cv.inRange( hsv, yl, yu )
mask_orange = cv.inRange( hsv, ol, ou )
# DISPLAY PREVIEW
window_name = name+" (normal)"
cv.imshow( window_name , input );
#cv.imshow( name+" (mask)" , mask );
cv.moveWindow( window_name, f_pos_x, f_pos_y );
# COUNT NUMBER OF WHITE PIXELS ON MASK (if mostly white this is our color)
nb_white_pix_red1 = np.sum(mask_red1 == 255);
nb_white_pix_red2 = np.sum(mask_red2 == 255);
nb_white_pix_green = np.sum(mask_green == 255);
nb_white_pix_blue = np.sum(mask_blue == 255);
nb_white_pix_white = np.sum(mask_white == 255);
nb_white_pix_yellow = np.sum(mask_yellow == 255);
nb_white_pix_orange = np.sum(mask_orange == 255);
tab_nb_white_pix = [
np.sum(mask_red1 == 255),
np.sum(mask_red2 == 255),
np.sum(mask_green == 255),
np.sum(mask_blue == 255),
np.sum(mask_white == 255),
np.sum(mask_yellow == 255),
np.sum(mask_orange == 255)
]
correspond = [
"red",
"red",
"green",
"blue",
"white",
"yellow",
"orange"
]
# FIND MOSTLY PRESENT COLOR
nb_mostly_present_color = tab_nb_white_pix.index( max(tab_nb_white_pix) )
color = correspond[nb_mostly_present_color]
# DEBUG
#print ( max(tab_nb_white_pix) ) # max value
#print ( nb_mostly_present_color ) # index of max value
print ( "Facette color : " + color ) # Color of the facette
return color
def convertColorsToKociemba(face_u,face_r,face_f,face_d,face_l,face_b):
# KOCIEMBA INPUT
# Below code convert face colors to Kociemba input
# = replace color with kociemba message
for i,c in enumerate(face_u):
if i!=0: # We keep reference [0]
if c == face_f[0] : face_u[i] = "F"
elif c == face_u[0] : face_u[i] = "U"
elif c == face_r[0] : face_u[i] = "R"
elif c == face_d[0] : face_u[i] = "D"
elif c == face_l[0] : face_u[i] = "L"
elif c == face_b[0] : face_u[i] = "B"
else: face_u[i] = "ERROR"
#
for i,c in enumerate(face_r):
if i!=0: # We keep reference [0]
if c == face_f[0] : face_r[i] = "F"
elif c == face_u[0] : face_r[i] = "U"
elif c == face_r[0] : face_r[i] = "R"
elif c == face_d[0] : face_r[i] = "D"
elif c == face_l[0] : face_r[i] = "L"
elif c == face_b[0] : face_r[i] = "B"
else: face_r[i] = "ERROR"
#
for i,c in enumerate(face_f):
if i!=0: # We keep reference [0]
if c == face_f[0] : face_f[i] = "F"
elif c == face_u[0] : face_f[i] = "U"
elif c == face_r[0] : face_f[i] = "R"
elif c == face_d[0] : face_f[i] = "D"
elif c == face_l[0] : face_f[i] = "L"
elif c == face_b[0] : face_f[i] = "B"
else: face_f[i] = "ERROR"
#
for i,c in enumerate(face_d):
if i!=0: # We keep reference [0]
if c == face_f[0] : face_d[i] = "F"
elif c == face_u[0] : face_d[i] = "U"
elif c == face_r[0] : face_d[i] = "R"
elif c == face_d[0] : face_d[i] = "D"
elif c == face_l[0] : face_d[i] = "L"
elif c == face_b[0] : face_d[i] = "B"
else: face_d[i] = "ERROR"
#
for i,c in enumerate(face_l):
if i!=0: # We keep reference [0]
if c == face_f[0] : face_l[i] = "F"
elif c == face_u[0] : face_l[i] = "U"
elif c == face_r[0] : face_l[i] = "R"
elif c == face_d[0] : face_l[i] = "D"
elif c == face_l[0] : face_l[i] = "L"
elif c == face_b[0] : face_l[i] = "B"
else: face_l[i] = "ERROR"
#
for i,c in enumerate(face_b):
if i!=0: # We keep reference [0]
if c == face_f[0] : face_b[i] = "F"
elif c == face_u[0] : face_b[i] = "U"
elif c == face_r[0] : face_b[i] = "R"
elif c == face_d[0] : face_b[i] = "D"
elif c == face_l[0] : face_b[i] = "L"
elif c == face_b[0] : face_b[i] = "B"
else: face_b[i] = "ERROR"
#
koc_colors = ""
#
for i,c in enumerate(face_u):
if i!=0: koc_colors = koc_colors+c
#
for i,c in enumerate(face_r):
if i!=0: koc_colors = koc_colors+c
#
for i,c in enumerate(face_f):
if i!=0: koc_colors = koc_colors+c
#
for i,c in enumerate(face_d):
if i!=0: koc_colors = koc_colors+c
#
for i,c in enumerate(face_l):
if i!=0: koc_colors = koc_colors+c
#
for i,c in enumerate(face_b):
if i!=0: koc_colors = koc_colors+c
#
print ("koc_colors -> "+ koc_colors) # LLRDUBRFR DRDDRUFLL FRFDFBUUU LLLFDFUFB FLUBLRRDB BBDRBUDUB
#
return koc_colors;
def rotate(axe,value):
# À faire
print ( "rotation axe = " + axe + " value = " + str(value) )
# Send rotation to Arduino (and motors)
time.sleep(1) # Wait while Arduino is doing the job
# Open port
#ser = serial.Serial("/dev/ttyUSB0",9600)
#ser.flushInput()
###############################################################################
################################# /functions ##################################
###############################################################################
##########################################################################
################################## MAIN ##################################
##########################################################################
# INIT
video_capture = cv.VideoCapture(0)
while(1):
print( "- - - - - - - - - - - - - - -" )
####################################
############## OPENCV ##############
####################################
# Facette size
f_w = int(rk_w/3) # facette width
f_h = int(rk_h/3) # facette height
# - - - - - - - -
_, frame = video_capture.read() # Take frame from the webcam | "_" is a variable | cap.read() return 2 values
tab_cropped_img = crop(frame) # Crop frame in 9 images
# - - - - - - - -
# FRONT
face_f[1] = findColor( tab_cropped_img[0], "1", f_w*0, 50 )
face_f[2] = findColor( tab_cropped_img[1], "2", f_w*1, 50 )
face_f[3] = findColor( tab_cropped_img[2], "3", f_w*2, 50 )
face_f[4] = findColor( tab_cropped_img[3], "4", f_w*0, 50+f_h )
face_f[5] = findColor( tab_cropped_img[4], "5", f_w*1, 50+f_h )
face_f[6] = findColor( tab_cropped_img[5], "6", f_w*2, 50+f_h )
face_f[7] = findColor( tab_cropped_img[6], "7", f_w*0, 50+2*f_h )
face_f[8] = findColor( tab_cropped_img[7], "8", f_w*1, 50+2*f_h )
face_f[9] = findColor( tab_cropped_img[8], "9", f_w*2, 50+2*f_h )
# - - - - - - - -
rotate("x",-1) # = go to down
_, frame = video_capture.read() # Take frame from the webcam | "_" is a variable | cap.read() return 2 values
tab_cropped_img = crop(frame) # Crop frame in 9 images
# - - - - - - - -
# DOWN
face_d[1] = findColor( tab_cropped_img[0], "1", f_w*0, 50, )
face_d[2] = findColor( tab_cropped_img[1], "2", f_w*1, 50, )
face_d[3] = findColor( tab_cropped_img[2], "3", f_w*2, 50, )
face_d[4] = findColor( tab_cropped_img[3], "4", f_w*0, 50+f_h, )
face_d[5] = findColor( tab_cropped_img[4], "5", f_w*1, 50+f_h, )
face_d[6] = findColor( tab_cropped_img[5], "6", f_w*2, 50+f_h, )
face_d[7] = findColor( tab_cropped_img[6], "7", f_w*0, 50+2*f_h, )
face_d[8] = findColor( tab_cropped_img[7], "8", f_w*1, 50+2*f_h, )
face_d[9] = findColor( tab_cropped_img[8], "9", f_w*2, 50+2*f_h, )
# - - - - - - - -
rotate("x",-1) # = go to back
_, frame = video_capture.read() # Take frame from the webcam | "_" is a variable | cap.read() return 2 values
tab_cropped_img = crop(frame) # Crop frame in 9 images
# - - - - - - - -
# BACK
face_b[1] = findColor( tab_cropped_img[0], "1", f_w*0, 50, )
face_b[2] = findColor( tab_cropped_img[1], "2", f_w*1, 50, )
face_b[3] = findColor( tab_cropped_img[2], "3", f_w*2, 50, )
face_b[4] = findColor( tab_cropped_img[3], "4", f_w*0, 50+f_h, )
face_b[5] = findColor( tab_cropped_img[4], "5", f_w*1, 50+f_h, )
face_b[6] = findColor( tab_cropped_img[5], "6", f_w*2, 50+f_h, )
face_b[7] = findColor( tab_cropped_img[6], "7", f_w*0, 50+2*f_h, )
face_b[8] = findColor( tab_cropped_img[7], "8", f_w*1, 50+2*f_h, )
face_b[9] = findColor( tab_cropped_img[8], "9", f_w*2, 50+2*f_h, )
# - - - - - - - -
rotate("x",-1) # = go to up
_, frame = video_capture.read() # Take frame from the webcam | "_" is a variable | cap.read() return 2 values
tab_cropped_img = crop(frame) # Crop frame in 9 images
# - - - - - - - -
# UP
face_u[1] = findColor( tab_cropped_img[0], "1", f_w*0, 50, )
face_u[2] = findColor( tab_cropped_img[1], "2", f_w*1, 50, )
face_u[3] = findColor( tab_cropped_img[2], "3", f_w*2, 50, )
face_u[4] = findColor( tab_cropped_img[3], "4", f_w*0, 50+f_h, )
face_u[5] = findColor( tab_cropped_img[4], "5", f_w*1, 50+f_h, )
face_u[6] = findColor( tab_cropped_img[5], "6", f_w*2, 50+f_h, )
face_u[7] = findColor( tab_cropped_img[6], "7", f_w*0, 50+2*f_h, )
face_u[8] = findColor( tab_cropped_img[7], "8", f_w*1, 50+2*f_h, )
face_u[9] = findColor( tab_cropped_img[8], "9", f_w*2, 50+2*f_h, )
# - - - - - - - -
rotate("x",-1) # = go to front
rotate("y",-1) # = go to left
_, frame = video_capture.read() # Take frame from the webcam | "_" is a variable | cap.read() return 2 values
tab_cropped_img = crop(frame) # Crop frame in 9 images
# - - - - - - - -
# LEFT
face_l[1] = findColor( tab_cropped_img[0], "1", f_w*0, 50, )
face_l[2] = findColor( tab_cropped_img[1], "2", f_w*1, 50, )
face_l[3] = findColor( tab_cropped_img[2], "3", f_w*2, 50, )
face_l[4] = findColor( tab_cropped_img[3], "4", f_w*0, 50+f_h, )
face_l[5] = findColor( tab_cropped_img[4], "5", f_w*1, 50+f_h, )
face_l[6] = findColor( tab_cropped_img[5], "6", f_w*2, 50+f_h, )
face_l[7] = findColor( tab_cropped_img[6], "7", f_w*0, 50+2*f_h, )
face_l[8] = findColor( tab_cropped_img[7], "8", f_w*1, 50+2*f_h, )
face_l[9] = findColor( tab_cropped_img[8], "9", f_w*2, 50+2*f_h, )
# - - - - - - - -
rotate("y",1) # = go to front
rotate("y",1) # = go to right
_, frame = video_capture.read() # Take frame from the webcam | "_" is a variable | cap.read() return 2 values
tab_cropped_img = crop(frame) # Crop frame in 9 images
# - - - - - - - -
# RIGHT
face_r[1] = findColor( tab_cropped_img[0], "1", f_w*0, 50, )
face_r[2] = findColor( tab_cropped_img[1], "2", f_w*1, 50, )
face_r[3] = findColor( tab_cropped_img[2], "3", f_w*2, 50, )
face_r[4] = findColor( tab_cropped_img[3], "4", f_w*0, 50+f_h, )
face_r[5] = findColor( tab_cropped_img[4], "5", f_w*1, 50+f_h, )
face_r[6] = findColor( tab_cropped_img[5], "6", f_w*2, 50+f_h, )
face_r[7] = findColor( tab_cropped_img[6], "7", f_w*0, 50+2*f_h, )
face_r[8] = findColor( tab_cropped_img[7], "8", f_w*1, 50+2*f_h, )
face_r[9] = findColor( tab_cropped_img[8], "9", f_w*2, 50+2*f_h, )
# - - - - - - - -
rotate("y",-1) # = go to front
# - - - - - - - -
####################################
############# KOCIEMBA #############
####################################
kociemba_colors = convertColorsToKociemba(face_u,face_r,face_f,face_d,face_l,face_b)
print (kociemba_colors)
try:
kociemba_resultat = kociemba.solve(kociemba_colors) # F2 L2 F2 D B R' U B L U2 B' U R2 F2 R2 L2 U2 R2 U' L2
except:
kociemba_resultat = "ERROR ( Finding bad colors from webcam )"
print ("kociemba_resultat -> " + kociemba_resultat)
# A single letter by itself means to turn that face clockwise 90 degrees.
# A letter followed by an apostrophe means to turn that face counterclockwise 90 degrees.
# A letter with the number 2 after it means to turn that face 180 degrees.
####################################
############ EXIT WHILE ############
####################################
k = cv.waitKey(5) & 0xFF
if k == 27:
break
##########################################################################
################################# /main ##################################
##########################################################################
# EXIT, destroy
cv.destroyAllWindows()
Slave Code (Arduino)
// - - - - - - 11 juin 2018 - - - - - -
#include <Servo.h>
/*
##########################################################################
################################ SETTINGS ################################
##########################################################################
*/
#define Servo_Pin1 5
#define Servo_Pin2 8
String InputString = "";
boolean StringComplete = false;
int servo_position;
Servo servo1, servo2;
#define srv_min_sig = 1000 // Servo min signal
#define srv_max_sig = 2000 // Servo max signal
#define srv_min_ang = 0 // Servo min angle
#define srv_max_ang = 180 // Servo max angle
/*
##########################################################################
############################### FUNCTIONS ################################
##########################################################################
*/
void setAngle(int angle) {
// Conversion - - - - -
/*
Objectif : Conversion de "plage"
Par exmeple : 0-180 vers 1000-2000
Calcul :
-- 1000-2000 devient 0-1000
-- 180 * x = 1000 <=> x = 1000/180
-- DONC signal = angle*1000/180+1000
*/
new_min = 0
new_max = srv_max_sig - srv_min_sig
servo_signal = angle * new_max / srv_max_ang + srv_min_sig
//conversion - - - - -
// À faire ->
//
//
}
/*
##########################################################################
################################# INIT ###################################
##########################################################################
*/
void setup() {
Serial.begin(9600);
servo1.attach(Servo_Pin1); delay(1000);
servo2.attach(Servo_Pin2); delay(1000);
servo1.writeMicroseconds(1000);
servo2.writeMicroseconds(1000);
}
/*
##########################################################################
############################## MAIN LOOP #################################
##########################################################################
*/
void loop() {
if( Serial.available(mesure) > 1 ) {
servo_position = Serial.print(mesure);
Serial.print(servo_position);
servo1.writeMicroseconds(servo_position);
servo2.writeMicroseconds(servo_position);
}
}