import copy
import requests
import threading
import pygame
import sys
# import twophase.solver as sv
# import kociemba
import pygame
import sys
import time
import sched
# simple class to improve readability of the counters within servos
class Counter:
servo = None
angle = None
delay = None
def __init__(self, n_servo, n_angle, n_delay):
self.servo = n_servo
self.angle = n_angle
self.delay = n_delay
class Move:
moveType = None
side = None
_validSides = {'b': 0, 'r': 1, 'f': 2, 'l': 3, 0: 0, 1: 1, 2: 2, 3: 3}
position = None
_validAngles = {0: 0, 1: 1, 2: 2, 3: 3}
_validPositions = {'f': 'f', 'e': 'e', 'r': 'r'}
delay = None
def __init__(self, _move_type, _side=None, _position=None, _delay=0.0):
self.type = _move_type
if _move_type == 'spin':
self.moveType = _move_type
try:
self.side = self._validSides.get(_side)
except KeyError:
raise ValueError("Invalid Side Num")
try:
self.position = self._validAngles.get(_position)
if self.position is None:
raise ValueError("But why")
except KeyError:
raise ValueError("Invalid Side Num")
elif _move_type == 'extension':
self.moveType = _move_type
try:
self.side = self._validSides.get(_side)
except KeyError:
raise ValueError("Invalid Side Num")
# quickly sanity check the input value
try:
self.position = self._validPositions.get(_position)
except KeyError:
raise ValueError("Position must be either f, e or r")
elif _move_type == 'delay':
self.moveType = _move_type
self.delay = _delay
elif _move_type == 'stop':
self.moveType = _move_type
self.side = _side
else:
raise ValueError("Invalid move type")
def execute(self, _machine):
if self.moveType == 'spin':
_machine.goto(self.side, self.position)
elif self.moveType == 'extension':
if self.position == 'e':
_machine.extend(self.side)
elif self.position == 'r':
_machine.retract(self.side)
elif self.position == 'f':
_machine.full_extend(self.side)
elif self.moveType == 'delay':
return self.delay
elif self.moveType == 'stop':
_machine.set_angle(self.side, None)
_machine.set_angle(self.side + 4, None)
return 0
class MoveList:
moves: list[Move] = []
moves2: list[Move] == []
counter = 1
machine = None
waitSeconds = 0
_running = False
def __init__(self, _machine, _moves: list[Move]):
self.machine = _machine,
self.moves = _moves
def execute(self):
self.moves2 = copy.copy(self.moves)
self._running = True
def tick(self):
if self._running:
self.waitSeconds -= .05
if self.waitSeconds <= 0:
while True:
try:
move_delay = self.moves2[0].execute(machine)
except IndexError:
self._running = False
self.counter -= 1
if self.counter != 0:
self.execute()
break
self.moves2.pop(0)
if move_delay > 0:
self.waitSeconds = move_delay
break
class Machine:
counters: list[Counter] = []
servoAngles = [0 for _ in range(9)]
servoPositions = [1, 1, 1, 1]
extraMoveDegrees = 5
rotator_servo_angles = {
0: [[172], [163], [159], [173]],
1: [[117], [111], [105], [122]],
2: [[62], [62], [53], [70]],
3: [[17], [10], [3], [16]],
}
retraction_servo_angles = {
'fullExtend': [150, 120, 120, 130],
'extend': [130, 110, 110, 115],
'retract': [73, 73, 73, 73],
}
# how long to wait after each length of move before turning the servo back
moveTimings = [0.1, 0.2, 0.35]
def tick(self):
for counter in self.counters:
if counter.delay > 0:
counter.delay -= .05
if counter.delay <= 0:
self.set_angle(counter.servo, counter.angle)
def set_angle(self, index, angle, delay=0.0):
if delay == 0:
if angle is None:
requests.post(url, data={
"component": "servo",
"index": index,
"value": -180,
})
else:
self.servoAngles[index] = angle
requests.post(url, data={
"component": "servo",
"index": index,
"value": angle,
})
else:
self.counters.append(Counter(index, angle, delay))
def goto(self, index, position):
# check which way the servo is moving and add a bit to account for hysteresis
angle = self.rotator_servo_angles[position][index][0]
if angle > self.servoAngles[index + 4]:
new_pos = angle + self.extraMoveDegrees
elif angle < self.servoAngles[index + 4]:
new_pos = angle - self.extraMoveDegrees
else:
new_pos = angle
delay = (abs(angle - self.servoAngles[index + 4]) / 700)
# set the position
self.set_angle(index + 4, new_pos)
# schedule a new move in 0.1s to move to the original position
self.set_angle(index + 4, angle, delay=delay + .1)
#
# self.set_angle(index + 4, angle)
# self.set_angle(index + 4, None, delay=delay+.3)
def move(self, index, move_by):
move_to = (self.servoPositions[index] + move_by) % 4
self.goto(index, move_to)
def retract(self, index):
self.set_angle(index, None, delay=0.5)
self.set_angle(index, self.retraction_servo_angles['retract'][index])
def extend(self, index):
self.set_angle(index, None, delay=0.5)
self.set_angle(index, self.retraction_servo_angles['extend'][index])
def full_extend(self, index):
self.set_angle(index, None, delay=0.5)
self.set_angle(index, self.retraction_servo_angles['fullExtend'][index])
if 'jank_fest':
class Cube:
faceLocation = {
"U": 4,
"D": 5,
"L": 1,
"R": 3,
"F": 0,
"B": 2
}
def rotateFB(self, invert):
values = list(self.faceLocation.values())
keys = list(self.faceLocation.keys())
# D -> L -> U -> R
if invert:
values = [values[3], values[2], values[1], values[0], values[4], values[5]]
self.faceLocation = dict(zip(keys, values))
print(self.faceLocation)
def rotateRL(self, invert):
values = list(self.faceLocation.values())
keys = list(self.faceLocation.keys())
# F -> D -> B -> U
if invert:
values = [values[5], values[4], values[2], values[3], values[0], values[1]]
self.faceLocation = dict(zip(keys, values))
print(self.faceLocation)
# __sssssssssssssss__
# translator dictionaries
npTr = {
"n": 1,
"p": -1
}
pnTr = {
"n": "p",
"p": "n"
}
if 'Setup Pygame':
def draw_text(text, x, y, color):
text_surface = font.render(text, True, color)
screen.blit(text_surface, (x, y))
# Function to check if a point is inside a rectangle
def is_mouse_over(pos, rect):
return rect.left < pos[0] < rect.right and rect.top < pos[1] < rect.bottom
# Function to create a button
def draw_button(rect, color, text):
pygame.draw.rect(screen, color, rect)
draw_text(text, rect.x + 10, rect.y + 10, BLACK)
class Button:
rect = pygame.Rect
isPressed = False
name = "1"
def draw(self, screen):
if self.isPressed:
pygame.draw.rect(screen, (0, 255, 0), self.rect)
else:
pygame.draw.rect(screen, (0, 0, 0), self.rect)
draw_text(self.name, self.rect.x + 10, self.rect.y + 10, (255, 255, 255))
# ___VARIABLES___
# timing stuff
scheduler = sched.scheduler(time.time, time.sleep)
scheduler.run()
# Network Stuff
pi_ip = '192.168.0.204'
url = "http://{0}:5000/receive".format(pi_ip)
# Pygame Stuff
# Define colors
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
GRAY = (150, 150, 150)
screen_width, screen_height = 400, 300
buttons = []
buttonPoss = [
(130, 10, 50, 50),
(250, 130, 50, 50),
(130, 250, 50, 50),
(10, 130, 50, 50),
(130, 70, 50, 50),
(190, 130, 50, 50),
(130, 190, 50, 50),
(70, 130, 50, 50),
]
# key grabbing
buttonPressed = 0
number_input = ""
# Misc
# Servos Class
machine = Machine()
# Initialize pygame
pygame.init()
# ___SETUP___
# Set up the screen
screen = pygame.display.set_mode((screen_width, screen_height))
pygame.display.set_caption("Simple GUI with Pygame")
# Create a font
font = pygame.font.SysFont(None, 40)
# Main function for GUI
if __name__ == "__main__":
delaydelay = 0.06
# setup gui
for i, pos in enumerate(buttonPoss):
buttons.append(Button())
buttons[i].rect = pygame.Rect(pos)
buttons[i].name = str(i)
extensionDelay = .2
delay2 = .2
solution = ["Rp", "Bn", "Bn", "Up", "Fn", "Fn", "Dn", "Fn", "Dn", "Bp", "Un", "Rp", "Un", "Un", "Rn", "Rn", "Dp",
"Fn", "Fn", "Un", "Un", "Fn", "Fn", "Rn", "Rn", "Un", "Un", "Rn", "Rn", "Fn", "Fn"]
servoMoves = MoveList(machine, [])
servoAngles = [1, -1, -1, 1]
num = 0
cube = Cube()
for move in solution:
faceNum = cube.faceLocation[move[0]]
# first, check if the move is on a face which is already in place
# if the face is not on the plane, perform a retraction
if faceNum > 3:
# todo: pick which way to do the retraction
cube.rotateRL(True)
# check if the servos to rotate are at the right angle
# if they are not, we need to retract and correct.
# this means we need to rotate the servo back
num += 1
# if either of the servos are wrong, retract them both and set them to position 1 and 2, respectively
if servoAngles[1] == 3 or servoAngles[3] == 0:
servoMoves.moves.append(Move('extension', 1, _position='r'))
servoMoves.moves.append(Move('extension', 3, _position='r'))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoMoves.moves.append(Move('spin', 1, _position=1))
servoMoves.moves.append(Move('spin', 3, _position=2))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoMoves.moves.append(Move('extension', 1, _position='e'))
servoMoves.moves.append(Move('extension', 3, _position='e'))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoAngles[0] = 1
servoAngles[2] = 2
servoAngles[1] = 1
servoAngles[3] = 2
# rotate the cube
# servoMoves.moves.append("0r00002r00501p00003n01002e00000e0150") # most of this timing seems to be perfect
servoMoves.moves.append(Move('extension', 0, _position='r'))
servoMoves.moves.append(Move('extension', 2, _position='r'))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoMoves.moves.append(Move('spin', 1, _position=servoAngles[1]+1))
servoMoves.moves.append(Move('spin', 3, _position=servoAngles[3]-1))
servoMoves.moves.append(Move('spin', 0, _position=1))
servoMoves.moves.append(Move('spin', 2, _position=2))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoMoves.moves.append(Move('extension', 0, _position='e'))
servoMoves.moves.append(Move('extension', 2, _position='e'))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoAngles[1] += 1
servoAngles[3] -= 1
# next, check if the servo is at the correct angle:
faceNum = cube.faceLocation[move[0]]
if abs(servoAngles[faceNum] + npTr[move[1]]) > 1:
# if the servo cannot perform the move, do a retraction and reset its rotation
servoMoves.moves.append(Move('extension', faceNum, _position='r'))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoMoves.moves.append(Move('spin', faceNum, _position=servoAngles[faceNum] - npTr[move[1]]))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoMoves.moves.append(Move('extension', faceNum, _position='e'))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoAngles[faceNum] -= npTr[move[1]]
servoMoves.moves.append(Move('spin', faceNum, _position=servoAngles[faceNum] + npTr[move[1]]))
servoMoves.moves.append(Move('delay', _delay=delaydelay))
servoAngles[faceNum] += npTr[move[1]]
print(servoMoves)
print(servoAngles)
# moves = MoveList(machine, [
# # retract opposite servos and set to positions
# Move('extension', 0, _position='r'),
# Move('extension', 2, _position='r'),
# Move('delay', _delay=.1),
# Move('spin', 0, _position=0),
# Move('spin', 2, _position=2),
# Move('delay', _delay=.5),
# Move('extension', 0, _position='e'),
# Move('extension', 2, _position='e'),
# Move('delay', _delay=extensionDelay),
# Move('extension', 1, _position='r'),
# Move('extension', 3, _position='r'),
# Move('spin', 0, _position=2),
# Move('spin', 2, _position=0),
# Move('spin', 1, _position=1),
# Move('spin', 3, _position=0),
# Move('delay', _delay=delay2),
# ##
# Move('extension', 1, _position='e'),
# Move('extension', 3, _position='e'),
# Move('delay', _delay=extensionDelay),
# Move('extension', 0, _position='r'),
# Move('extension', 2, _position='r'),
# Move('spin', 1, _position=0),
# Move('spin', 3, _position=1),
# Move('spin', 0, _position=1),
# Move('spin', 2, _position=0),
# Move('delay', _delay=delay2),
# ##
# Move('extension', 0, _position='e'),
# Move('extension', 2, _position='e'),
# Move('delay', _delay=extensionDelay),
# Move('extension', 1, _position='r'),
# Move('extension', 3, _position='r'),
# Move('spin', 1, _position=1),
# Move('spin', 3, _position=0),
# Move('spin', 0, _position=0),
# Move('spin', 2, _position=1),
# Move('delay', _delay=delay2),
# ##
# Move('extension', 1, _position='e'),
# Move('extension', 3, _position='e'),
# Move('delay', _delay=extensionDelay),
# Move('extension', 0, _position='r'),
# Move('extension', 2, _position='r'),
# Move('spin', 1, _position=0),
# Move('spin', 3, _position=1),
# Move('spin', 0, _position=1),
# Move('spin', 2, _position=0),
# Move('delay', _delay=delay2),
# ##
# Move('extension', 0, _position='e'),
# Move('extension', 2, _position='e'),
# Move('delay', _delay=extensionDelay),
# Move('extension', 1, _position='r'),
# Move('extension', 3, _position='r'),
# Move('spin', 1, _position=1),
# Move('spin', 3, _position=0),
# Move('spin', 0, _position=0),
# Move('spin', 2, _position=1),
# Move('delay', _delay=delay2),
# Move('extension', 1, _position='e'),
# Move('extension', 3, _position='e'),
# Move('delay', _delay=1),
# Move('stop', 0),
# Move('stop', 1),
# Move('stop', 2),
# Move('stop', 3),
# ])
moves1 = MoveList(machine, [
Move('extension', 0, _position='r'),
Move('extension', 2, _position='r'),
Move('delay', _delay=.1),
Move('spin', 0, _position=0),
Move('spin', 2, _position=0),
Move('delay', _delay=.5),
Move('extension', 0, _position='e'),
Move('extension', 2, _position='e'),
Move('delay', _delay=extensionDelay),
# Move('extension', 1, _position='r'),
# Move('extension', 3, _position='r'),
# Move('spin', 1, _position=0),
# Move('spin', 3, _position=0),
# Move('delay', _delay=extensionDelay),
# Move('extension', 1, _position='e'),
# Move('extension', 3, _position='e'),
])
moves2 = MoveList(machine, [
Move('spin', 0, _position=1),
Move('delay', _delay=delaydelay),
Move('spin', 1, _position=1),
Move('delay', _delay=delaydelay),
Move('spin', 2, _position=1),
Move('delay', _delay=delaydelay),
Move('spin', 3, _position=1),
Move('delay', _delay=delaydelay),
Move('spin', 0, _position=0),
Move('delay', _delay=delaydelay),
Move('spin', 1, _position=0),
Move('delay', _delay=delaydelay),
Move('spin', 2, _position=0),
Move('delay', _delay=delaydelay),
Move('spin', 3, _position=0),
Move('delay', _delay=delaydelay),
])
clock = pygame.time.Clock()
moves1.execute()
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_RETURN:
servoMoves.execute()
# try:
# num = int(number_input)
# moves2.counter = num
# moves2.execute()
# except:
# pass
# machine.set_angle(buttonPressed, number_input)
# # print("Entered number:", number_input)
# data_to_send = {
# "component" : "servo",
# "index": buttonPressed,
# "value": number_input,
# }
number_input = ""
# response = requests.post(url, data=data_to_send)
elif event.key == pygame.K_BACKSPACE:
number_input = number_input[:-1]
elif event.key == pygame.K_UP:
buttonPressed = 2
elif event.key == pygame.K_RIGHT:
buttonPressed = 3
elif event.key == pygame.K_DOWN:
buttonPressed = 0
elif event.key == pygame.K_LEFT:
buttonPressed = 1
else:
number_input += event.unicode
if buttonPressed < 4:
if event.key == pygame.K_r:
machine.retract(buttonPressed)
elif event.key == pygame.K_e:
machine.extend(buttonPressed)
elif event.key == pygame.K_t:
machine.full_extend(buttonPressed)
elif event.key == pygame.K_a:
machine.goto(buttonPressed, 0)
elif event.key == pygame.K_s:
machine.goto(buttonPressed, 1)
elif event.key == pygame.K_d:
machine.goto(buttonPressed, 2)
elif event.key == pygame.K_f:
machine.goto(buttonPressed, 3)
elif event.key == pygame.K_q:
machine.move(buttonPressed, 1)
elif event.key == pygame.K_w:
machine.move(buttonPressed, -1)
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
mouse_pos = pygame.mouse.get_pos()
for i, button in enumerate(buttons):
button.isPressed = False
if is_mouse_over(mouse_pos, button.rect):
buttonPressed = i
button.isPressed = True
# # Check if the left mouse button is clicked on the buttons
if is_mouse_over(mouse_pos, pygame.Rect(150, 150, 100, 50)):
print("Button 1 clicked!")
elif is_mouse_over(mouse_pos, pygame.Rect(150, 220, 100, 50)):
print("Button 2 clicked!")
# tick the servos
machine.tick()
moves1.tick()
moves2.tick()
servoMoves.tick()
# Clear the screen
screen.fill(WHITE)
# Draw the input field
pygame.draw.rect(screen, GRAY, pygame.Rect(10, 10, 200, 40))
draw_text(number_input, 20, 20, BLACK)
# Draw buttons
for button in buttons:
button.draw(screen)
# draw_button(pygame.Rect(150, 150, 100, 50), BLACK, "Button 1")
# draw_button(pygame.Rect(150, 220, 100, 50), BLACK, "Button 2")
# Update the display
pygame.display.flip()
clock.tick(20)
