diff --git a/.gitignore b/.gitignore index d9005f2..da7e738 100644 --- a/.gitignore +++ b/.gitignore @@ -150,3 +150,4 @@ cython_debug/ # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ +.vscode/ \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..ce7ef18 --- /dev/null +++ b/README.md @@ -0,0 +1,30 @@ +# autoPlanner + +(WIP) An auto creation tool for Ridgebotics 2024 + +### Install +```shell +git clone https://github.com/astatin3/autoPlanner +cd autoPlanner +pip install -r requirements.txt +python3 ./main.py +``` +### Usage: + +##### "Path Editor" Tab: +- Click to add nodes +- Click on specific points to manipulate paths and nodes + +##### "Button editor" Tab: +- Click on specific frames on the timeline to change to that position +- When selected on a frame, the robot's position in that time should show up. +- Drag positional keyframes around to speed up and speed down the robot's travel between nodes +- While a frame is selected, Press the 'e' key to swap to button mode. +- In button mode select buttons on the driver and operator controllers. + +##### "Export" Tab: +- Click export, and save to a file + +### Known Bugs: +- Because the variables don't get transferred over yet, you must click on the button editor tab before exporting. +- The driver controller's movement stick is rotated by 90 degrees (Maybe) \ No newline at end of file diff --git a/images/Field.png b/images/Field.png new file mode 100644 index 0000000..4a384db Binary files /dev/null and b/images/Field.png differ diff --git a/images/XboxOne_A.png b/images/XboxOne_A.png new file mode 100644 index 0000000..28c3b60 Binary files /dev/null and b/images/XboxOne_A.png differ diff --git a/images/XboxOne_B.png b/images/XboxOne_B.png new file mode 100644 index 0000000..2a4d9b6 Binary files /dev/null and b/images/XboxOne_B.png differ diff --git a/images/XboxOne_Diagram.png b/images/XboxOne_Diagram.png new file mode 100644 index 0000000..9b79c5d Binary files /dev/null and b/images/XboxOne_Diagram.png differ diff --git a/images/XboxOne_Diagram_Simple.png b/images/XboxOne_Diagram_Simple.png new file mode 100644 index 0000000..7e3a1d9 Binary files /dev/null and b/images/XboxOne_Diagram_Simple.png differ diff --git a/images/XboxOne_Dpad.png b/images/XboxOne_Dpad.png new file mode 100644 index 0000000..9c04c85 Binary files /dev/null and b/images/XboxOne_Dpad.png differ diff --git a/images/XboxOne_Dpad_Down.png b/images/XboxOne_Dpad_Down.png new file mode 100644 index 0000000..8c1134c Binary files /dev/null and b/images/XboxOne_Dpad_Down.png differ diff --git a/images/XboxOne_Dpad_Left.png b/images/XboxOne_Dpad_Left.png new file mode 100644 index 0000000..45ddb16 Binary files /dev/null and b/images/XboxOne_Dpad_Left.png differ diff --git a/images/XboxOne_Dpad_Right.png b/images/XboxOne_Dpad_Right.png new file mode 100644 index 0000000..a1fbc2c Binary files /dev/null and b/images/XboxOne_Dpad_Right.png differ diff --git a/images/XboxOne_Dpad_Up.png b/images/XboxOne_Dpad_Up.png new file mode 100644 index 0000000..e8eab7d Binary files /dev/null and b/images/XboxOne_Dpad_Up.png differ diff --git a/images/XboxOne_LB.png b/images/XboxOne_LB.png new file mode 100644 index 0000000..f6c414b Binary files /dev/null and b/images/XboxOne_LB.png differ diff --git a/images/XboxOne_LT.png b/images/XboxOne_LT.png new file mode 100644 index 0000000..e9681c9 Binary files /dev/null and b/images/XboxOne_LT.png differ diff --git a/images/XboxOne_Left_Stick.png b/images/XboxOne_Left_Stick.png new file mode 100644 index 0000000..9c5ca5c Binary files /dev/null and b/images/XboxOne_Left_Stick.png differ diff --git a/images/XboxOne_Left_Stick_Click.png b/images/XboxOne_Left_Stick_Click.png new file mode 100644 index 0000000..ad0428f Binary files /dev/null and b/images/XboxOne_Left_Stick_Click.png differ diff --git a/images/XboxOne_Menu.png b/images/XboxOne_Menu.png new file mode 100644 index 0000000..2cbfb08 Binary files /dev/null and b/images/XboxOne_Menu.png differ diff --git a/images/XboxOne_RB.png b/images/XboxOne_RB.png new file mode 100644 index 0000000..5dcfc6d Binary files /dev/null and b/images/XboxOne_RB.png differ diff --git a/images/XboxOne_RT.png b/images/XboxOne_RT.png new file mode 100644 index 0000000..7bf27f4 Binary files /dev/null and b/images/XboxOne_RT.png differ diff --git a/images/XboxOne_Right_Stick.png b/images/XboxOne_Right_Stick.png new file mode 100644 index 0000000..3d83a22 Binary files /dev/null and b/images/XboxOne_Right_Stick.png differ diff --git a/images/XboxOne_Right_Stick_Click.png b/images/XboxOne_Right_Stick_Click.png new file mode 100644 index 0000000..de08508 Binary files /dev/null and b/images/XboxOne_Right_Stick_Click.png differ diff --git a/images/XboxOne_Windows.png b/images/XboxOne_Windows.png new file mode 100644 index 0000000..3756d29 Binary files /dev/null and b/images/XboxOne_Windows.png differ diff --git a/images/XboxOne_X.png b/images/XboxOne_X.png new file mode 100644 index 0000000..a0f0ac1 Binary files /dev/null and b/images/XboxOne_X.png differ diff --git a/images/XboxOne_Y.png b/images/XboxOne_Y.png new file mode 100644 index 0000000..4e31627 Binary files /dev/null and b/images/XboxOne_Y.png differ diff --git a/main.py b/main.py new file mode 100644 index 0000000..0ca8802 --- /dev/null +++ b/main.py @@ -0,0 +1,112 @@ +import math +from copy import copy + +import pygame as pg +from pygame.locals import * +from sys import exit +import numpy as np + +import src.render as render + +import src.pathEditor as pathEditor +import src.buttonEditor as buttonEditor +import src.export as export + +doubleClickDuration = 200 + +pg.init() +pg.font.init() + +topBarHeight = 40 +bottomBarHeight = 60 + +screen_width = 1200 +screen_height = (screen_width * (643/1286)) + topBarHeight + bottomBarHeight + +screen = pg.display.set_mode((screen_width, screen_height))#, pg.RESIZABLE) +pg.display.set_caption("Auto Planner") + +render = render.render(pg, screen, topBarHeight, bottomBarHeight) + +tabIndex = 0 +tabs = [ + pathEditor.pathEditor(render), + buttonEditor.buttonEditor(render, pathEditor), + export.export(pg, render, buttonEditor) +] + +tabs[tabIndex].load() + +def addTab(i): + x1 = i * (screen_width/(len(tabs))) + x2 = (screen_width/(len(tabs))) + rect = (x1, 0, x2, topBarHeight) + + def getIsSelected(): + global tabIndex + return tabIndex == i + + def getIsVisible(): + return True + + def onClick(pos): + global tabIndex + tabs[tabIndex].unload() + tabIndex = i + tabs[tabIndex].load() + render.renderElements(pos) + pg.display.update() + + render.addButton(rect, tabs[i].name, getIsSelected, getIsVisible, onClick) + +for i in range(len(tabs)): + addTab(i) + +render.renderElements((screen_width/2, screen_height/2)) + +running = True +last_click = -1 + +def offsetPos(pos): + return (pos[0],pos[1]) + +while running: + for event in pg.event.get(): + + if event.type == pg.MOUSEMOTION: + pos = pg.mouse.get_pos() + render.renderElements(pos) + if pos[1] > topBarHeight: + tabs[tabIndex].mouseMove(offsetPos(pos)) + # refreshTabs(pos) + + elif event.type == pg.MOUSEBUTTONDOWN: + pos = pg.mouse.get_pos() + render.clickElement(pos) + if pos[1] > topBarHeight: + now = pg.time.get_ticks() + if now - last_click <= doubleClickDuration: + tabs[tabIndex].doubleClick(offsetPos(pos)) + else: + tabs[tabIndex].mouseDown(offsetPos(pos)) + last_click = pg.time.get_ticks() + # else: + # clickTab(pos) + + elif event.type == pg.MOUSEBUTTONUP: + pos = pg.mouse.get_pos() + if pos[1] > topBarHeight: + tabs[tabIndex].mouseUp(offsetPos(pos)) + + elif event.type == pg.KEYDOWN: + tabs[tabIndex].keyDown(event.key) + if event.key == pg.K_TAB: + tabs[tabIndex].unload() + tabIndex = (tabIndex + 1) % len(tabs) + tabs[tabIndex].load() + render.renderElements(pg.mouse.get_pos()) + + elif event.type == pg.QUIT: + running = False + +pg.quit() diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..2806a78 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +numpy +pygame +crossfiledialog \ No newline at end of file diff --git a/src/buttonEditor.py b/src/buttonEditor.py new file mode 100644 index 0000000..aff2cf8 --- /dev/null +++ b/src/buttonEditor.py @@ -0,0 +1,654 @@ +import math +import copy +import json + +render = None +pathEditor = None +bottomBarRect = None + +# leftSidee = True + +ogNodes = [] +ogCtrlNodes = [] +ogRotNodes = [] + +keyFrames = [] + +matchLength = 15 +TPS = 50 + +tickTime = round(1/TPS*1000) +matchTicks = matchLength * TPS +displayTickResolution = 4 +displayTicks = round(matchTicks / displayTickResolution) + +buttonEditColor = (191,0,191) +buttonEditNodeRadius = 6 + +dragFrameIndex = -1 +ogDragFramePos = -1 + +selFrame = -1 + +buttonImages = {} +buttonMode = False + +buttonPositions = { + 'A': ((1089,494),100), + 'B': ((1187,404),100), + 'X': ((996,411),100), + 'Y': ((1093,321),100), + + 'Dpad': ((549,619),220), + + 'Dpad_Up': ((549,561),70), + 'Dpad_Down': ((549,677),70), + 'Dpad_Left': ((485,619),70), + 'Dpad_Right': ((607,619),70), + + 'Menu': ((832,411),100), + 'Windows': ((629,411),100), + + 'Left_Stick': ((375,422),150), + 'Right_Stick': ((914,622),150), + + 'LB': ((352,184),150), + 'RB': ((1100,184),150), + + 'LT': ((356,67),150), + 'RT': ((1096,67),150) +} + + + +def getKeyframeAtPos(index): + for frame in keyFrames: + if frame["timeIndex"] == index: + return frame + return None + + + +def getFrameIndex(frame): + if frame == None: + return -1 + return keyFrames.index(frame) + + + +def getPosKeyframeAtPos(index): + for frame in keyFrames: + if frame["timeIndex"] == index and frame['type'] == 'position': + return frame + return None + + + +def getPosKeyframes(): + frames = [] + for keyFrame in keyFrames: + if keyFrame['type'] == 'position': + frames.append(keyFrame) + return frames + + + +def getButtonKeyframes(): + frames = [] + for keyFrame in keyFrames: + if keyFrame['type'] == 'controller': + frames.append(keyFrame) + return frames + + + +def getBezierPointCounts(): + counts = [] + frames = getPosKeyframes() + for i in range(1,len(frames)): + counts.append(frames[i]['timeIndex'] - frames[i-1]['timeIndex']) + return counts + + + +def getPosKeyframeByIndex(index): + for frame in keyFrames: + if frame['type'] == 'position' and frame["index"] == index: + return frame + return None + + + +def getSurroundingPosFrames(index): + prevFrame = None + for i in range(index,-1,-1): + frame = getPosKeyframeAtPos(i) + if frame != None and (dragFrameIndex == -1 or not frame == keyFrames[dragFrameIndex]): + prevFrame = frame + break + nextFrame = None + for i in range(index,displayTicks,1): + frame = getPosKeyframeAtPos(i) + if frame != None and (dragFrameIndex == -1 or not frame == keyFrames[dragFrameIndex]): + nextFrame = frame + break + + if nextFrame == None and prevFrame == None: + return prevFrame, nextFrame + # elif nextFrame == None: + # return prevFrame, prevFrame + # elif prevFrame == None: + # return nextFrame, nextFrame + + return prevFrame, nextFrame + + + +def getLeftButtonFrame(index): + for i in range(index,0,-1): + frame = getKeyframeAtPos(i) + if frame != None and frame['type'] == 'controller': + return frame + return None + + + +def getButtonFrameAtPos(index): + for i in range(len(keyFrames)): + frame = keyFrames[i] + if frame != None and frame['type'] == 'controller': + return frame + return None + + + + +def getRobotAtIndex(index): + prevFrame, nextFrame = getSurroundingPosFrames(index) + + # print(prevFrame) + # print(nextFrame) + + if prevFrame == None and nextFrame == None: + return (0,0), 0 + if prevFrame == None: + return nextFrame['position'], nextFrame['rotation'] + elif nextFrame == None: + return prevFrame['position'], prevFrame['rotation'] + elif nextFrame['timeIndex'] - prevFrame['timeIndex'] == 0: + return prevFrame['position'], prevFrame['rotation'] + + relPos = -((prevFrame['timeIndex'] - index)/(nextFrame['timeIndex'] - prevFrame['timeIndex'])) + + pos = calcBezierPoint(prevFrame['position'], ogCtrlNodes[prevFrame['index']], nextFrame['position'], relPos) + + if prevFrame['rotation'] - nextFrame['rotation'] < -math.pi: + rot = ((nextFrame['rotation']-prevFrame['rotation']-math.pi*2)*relPos) + prevFrame['rotation'] + elif prevFrame['rotation'] - nextFrame['rotation'] > math.pi: + rot = ((nextFrame['rotation']-prevFrame['rotation']+math.pi*2)*relPos) + prevFrame['rotation'] + else: + rot = ((nextFrame['rotation']-prevFrame['rotation'])*relPos) + prevFrame['rotation'] + + # diff = (nextFrame['rotation']-prevFrame['rotation']) + # if diff >= math.pi: + # rot = ((nextFrame['rotation']-prevFrame['rotation']-math.pi*2)*relPos) + prevFrame['rotation'] + # elif diff <= math.pi: + # rot = ((nextFrame['rotation']-prevFrame['rotation']+math.pi*2)*relPos) + prevFrame['rotation'] + # else: + # rot = ((nextFrame['rotation']-prevFrame['rotation'])*relPos) + prevFrame['rotation'] + + + return pos, rot + + + +# def getTimeBarColor(index): +# frame = getKeyframeAtPos(index) +# if frame == None: +# return (0,0,0) +# if frame['type'] == 'position': +# return (127,127,0) +# elif frame['type'] == 'controller': +# return buttonEditColor + +# return (16,16,32) + + + +def calcBezierPoint(p0, p1, p2, t): + px = p0[0]*(1-t)**2 + 2*(1-t)*t*p1[0] + p2[0]*t**2 + py = p0[1]*(1-t)**2 + 2*(1-t)*t*p1[1] + p2[1]*t**2 + return (px, py) + + + + +def reloadBar(pos): + toggle = False + for i in range(displayTicks): + x1 = i * (render.width/(displayTicks)) + x2 = (render.width/(displayTicks)) + rect = (x1, bottomBarRect[1], x2, bottomBarRect[3]) + + color = (0, 0, 0) + + if i == selFrame: + color = (color[0]+64,color[1]+64,color[2]+64) + if render.isInRect(pos, rect): + color = (color[0]+64,color[1]+64,color[2]+64) + if dragFrameIndex != -1 and getKeyframeAtPos(i) == None: + if keyFrames[dragFrameIndex]['type'] == 'position': + prevFrame, nextFrame = getSurroundingPosFrames(ogDragFramePos) + + if prevFrame == nextFrame: + pass + elif prevFrame == None: + if i < nextFrame['timeIndex']: + keyFrames[dragFrameIndex]['timeIndex'] = i + elif nextFrame == None: + if i > prevFrame['timeIndex']: + keyFrames[dragFrameIndex]['timeIndex'] = i + elif i > prevFrame['timeIndex'] and i < nextFrame['timeIndex']: + keyFrames[dragFrameIndex]['timeIndex'] = i + + else: + keyFrames[dragFrameIndex]['timeIndex'] = i + else: + color = (color[0]+16+(toggle*16),color[1]+16+(toggle*16),color[2]+32+(toggle*16)) + + + frame = getKeyframeAtPos(i) + if frame == None: + pass + elif frame['type'] == 'position': + color = (191,191,0) + elif frame['type'] == 'controller': + color = buttonEditColor + + toggle = not toggle + + render.drawrect(color, rect) + # renderSelectIndicator(i) + render.update() + + + +def clickBar(pos): + for i in range(displayTicks): + x1 = i * (render.width/(displayTicks)) + x2 = (render.width/(displayTicks)) + rect = (x1, bottomBarRect[1], x2, bottomBarRect[3]) + + if render.isInRect(pos, rect): + global selFrame + global dragFrameIndex + global ogDragFramePos + selFrame = i + if dragFrameIndex == -1: + dragFrameIndex = getFrameIndex(getKeyframeAtPos(i)) + ogDragFramePos = i + return + + + +def createBlankController(): + returnArr = [] + for i in range(len(controllerRects)): + returnArr.append({ + 'A': False, + 'B': False, + 'X': False, + 'Y': False, + 'Dpad_Up': False, + 'Dpad_Down': False, + 'Dpad_Left': False, + 'Dpad_Right': False, + 'Menu': False, + 'Windows': False, + 'Left_Stick': False, + 'Right_Stick': False, + 'LB': False, + 'RB': False, + 'LT': False, + 'RT': False + }) + return returnArr + + + + +def toggleControllerButton(btnStr, controllerIndex): + global keyFrames + lastFrame = getLeftButtonFrame(selFrame) + if lastFrame == None: + keyFrames.append({ + "type": "controller", + "timeIndex": selFrame, + "controllers": createBlankController() + }) + frame = keyFrames[len(keyFrames)-1] + elif lastFrame['timeIndex'] != selFrame: + keyFrames.append({ + "type": "controller", + "timeIndex": selFrame, + "controllers": copy.deepcopy(lastFrame['controllers']) + }) + frame = keyFrames[len(keyFrames)-1] + else: + frame = lastFrame + + if not btnStr in ['Dpad_Up', 'Dpad_Down', 'Dpad_Left', 'Dpad_Right']: + + frame['controllers'][controllerIndex][btnStr] = not frame['controllers'][controllerIndex][btnStr] + + # Dpad Stuff + elif frame['controllers'][controllerIndex][btnStr] == True: + for btn in ['Dpad_Up', 'Dpad_Down', 'Dpad_Left', 'Dpad_Right']: + frame['controllers'][controllerIndex][btn] = False + else: + for btn in ['Dpad_Up', 'Dpad_Down', 'Dpad_Left', 'Dpad_Right']: + frame['controllers'][controllerIndex][btn] = False + frame['controllers'][controllerIndex][btnStr] = True + + + + + +def getControllerButtons(controllerIndex): + frame = getLeftButtonFrame(selFrame) + if frame == None: + return createBlankController()[0] + else: + return frame['controllers'][controllerIndex] + + + +def renderXboxControllers(): + for i in range(len(controllerRects)): + + rect = controllerRects[i] + + offsetSize = rect[2]/buttonImages['Controller'].get_width() + + def offsetControllerButton(index): + pos, size = buttonPositions[index] + rect2 = ((pos[0]-(size/2), pos[1]-(size/2), size, size)) + return (rect[0]+(rect2[0])*offsetSize,rect[1]+(rect2[1])*offsetSize,rect2[2]*offsetSize,rect2[2]*offsetSize) + + render.image(buttonImages['Controller'], rect) + + btns = getControllerButtons(i) + + for btn in ['A','B','X','Y','Menu','Windows','LB','RB','LT','RT','Left_Stick','Right_Stick']: + if btns[btn]: + render.image(render.invert(buttonImages[btn]), offsetControllerButton(btn)) + else: + render.image(buttonImages[btn], offsetControllerButton(btn)) + + if btns['Dpad_Up']: + render.image(buttonImages['Dpad_Up'], offsetControllerButton('Dpad')) + elif btns['Dpad_Down']: + render.image(buttonImages['Dpad_Down'], offsetControllerButton('Dpad')) + elif btns['Dpad_Left']: + render.image(buttonImages['Dpad_Left'], offsetControllerButton('Dpad')) + elif btns['Dpad_Right']: + render.image(buttonImages['Dpad_Right'], offsetControllerButton('Dpad')) + else: + render.image(buttonImages['Dpad'], offsetControllerButton('Dpad')) + + + # for btn in ['Dpad_Up','Dpad_Down','Dpad_Left','Dpad_Right']: + # if + # render.drawrect((255,255,255), offsetControllerButton(btn)) + + + +def controllerClick(pos): + for i in range(len(controllerRects)): + + rect = controllerRects[i] + + offsetSize = rect[2]/buttonImages['Controller'].get_width() + + def offsetControllerButton(index): + pos, size = buttonPositions[index] + rect2 = ((pos[0]-(size/2), pos[1]-(size/2), size, size)) + return (rect[0]+(rect2[0])*offsetSize,rect[1]+(rect2[1])*offsetSize,rect2[2]*offsetSize,rect2[2]*offsetSize) + + for btn in ['A','B','X','Y','Menu','Windows','LB','RB','LT','RT','Left_Stick','Right_Stick','Dpad_Up','Dpad_Down','Dpad_Left','Dpad_Right']: + if render.isInRect(pos, offsetControllerButton(btn)): + toggleControllerButton(btn, i) + + + + +def renderTimeText(): + if selFrame == -1: + return + seconds = round((((selFrame*displayTickResolution)+1)/matchTicks)*matchLength,2) + text = f'{str(seconds)} s / {str(matchLength)}.0 s' + + text = render.font.render(text, True, (255,255,255)) + + # global leftSide + + # if leftSide: + # rect = text.get_rect(bottomright=(render.width,render.height+render.topBarHeight)) + # else: + rect = text.get_rect(bottomleft=(0,render.height+render.topBarHeight)) + + render.screen.blit(text, rect) + + + + +class buttonEditor: + name = "Button Editor" + + def __init__(self, tmprender, tmppathEditor): + global render + global pathEditor + render = tmprender + pathEditor = tmppathEditor + + global indicatorBarHeight + indicatorBarHeight = round(render.screen.get_width()/displayTicks) + + global bottomBarRect + bottomBarRect = (0, (render.screen.get_height()-render.bottomBarHeight), render.screen.get_width(), render.bottomBarHeight) + + global buttonImages + buttonImages = { + "Controller": render.loadImg('images/XboxOne_Diagram_Simple.png'), + + "A": render.loadImg('images/XboxOne_A.png'), + "B": render.loadImg('images/XboxOne_B.png'), + "X": render.loadImg('images/XboxOne_X.png'), + "Y": render.loadImg('images/XboxOne_Y.png'), + + "Dpad": render.loadImg('images/XboxOne_Dpad.png'), + "Dpad_Up": render.loadImg('images/XboxOne_Dpad_Up.png'), + "Dpad_Down": render.loadImg('images/XboxOne_Dpad_Down.png'), + "Dpad_Left": render.loadImg('images/XboxOne_Dpad_Left.png'), + "Dpad_Right": render.loadImg('images/XboxOne_Dpad_Right.png'), + + "Menu": render.loadImg('images/XboxOne_Menu.png'), + "Windows": render.loadImg('images/XboxOne_Windows.png'), + + "Left_Stick": render.loadImg('images/XboxOne_Left_Stick.png'), + "Left_Stick_Click": render.loadImg('images/XboxOne_Left_Stick_Click.png'), + "Right_Stick": render.loadImg('images/XboxOne_Right_Stick.png'), + "Right_Stick_Click": render.loadImg('images/XboxOne_Right_Stick_Click.png'), + + + "LB": render.loadImg('images/XboxOne_LB.png'), + "RB": render.loadImg('images/XboxOne_RB.png'), + "LT": render.loadImg('images/XboxOne_LT.png'), + "RT": render.loadImg('images/XboxOne_RT.png') + + } + + ControllerSize = (render.width/2, render.width*(buttonImages['Controller'].get_height()/buttonImages['Controller'].get_width())/2) + ControllerYOffset = (render.height-ControllerSize[1])/2 + global controllerRects + controllerRects = [ + (0, render.topBarHeight+ControllerYOffset, ControllerSize[0], ControllerSize[1]), + (ControllerSize[0], render.topBarHeight+ControllerYOffset, ControllerSize[0], ControllerSize[1]) + ] + + def refresh(self): + render.clear() + if not buttonMode: + global ogNodes + global ogCtrlNodes + global ogRotNodes + + render.drawField() + + pointCounts = getBezierPointCounts() + for i in range(0,len(ogCtrlNodes)): + render.bezier(ogNodes[i], ogCtrlNodes[i], ogNodes[i+1], pointCounts[i]) + + buttonFrames = getButtonKeyframes() + for frame in buttonFrames: + pos, rot = getRobotAtIndex(frame['timeIndex']) + render.circle(buttonEditColor, pos, buttonEditNodeRadius) + + if selFrame != -1 and len(ogNodes) > 0: + pos, rot = getRobotAtIndex(selFrame) + render.robotSquare(pos, rot) + + + else: + renderXboxControllers() + + renderTimeText() + + reloadBar((0,0)) + render.update() + + + + def mouseDown(self, pos): + if buttonMode and pos[1] < bottomBarRect[1]: + controllerClick(pos) + self.refresh() + elif pos[1] > bottomBarRect[1]: + clickBar(pos) + self.refresh() + + + + def mouseUp(self, pos): + global dragFrameIndex + if dragFrameIndex != -1: + dragFrameIndex = -1 + ogDragFramePos = -1 + self.refresh() + reloadBar((0, 0)) + + + + def mouseMove(self, pos): + global dragFrameIndex + if dragFrameIndex != -1 or pos[1] > bottomBarRect[1]: + reloadBar(pos) + + # global leftSide + + # if leftSide and pos[0] > (render.width/2): + # leftSide = False + # self.refresh() + # if not leftSide and pos[0] < (render.width/2): + # leftSide = True + # self.refresh() + + # if pos[1] > bottomBarRect[1]: + + + + def doubleClick(self, pos): + pass + # if pos[1] > bottomBarRect[1]: + # clickBar(pos) + # self.refresh() + + + def keyDown(self, key): + global selFrame + global buttonMode + if key == render.pg.K_LEFT and selFrame > 0: + selFrame -= 1 + self.refresh() + elif key == render.pg.K_RIGHT and selFrame < displayTicks-1: + selFrame += 1 + self.refresh() + elif buttonMode and key == render.pg.K_DELETE and selFrame != -1: + frame = getKeyframeAtPos(selFrame) + if frame != None and frame['type'] != 'position': + global keyFrames + keyFrames.remove(frame) + self.refresh() + elif selFrame != -1 and key == render.pg.K_e: + buttonMode = not buttonMode + self.refresh() + + + def updateNodes(self, loadKeyframes): + global ogNodes + global ogCtrlNodes + global ogRotNodes + ogNodes = pathEditor.nodes.copy() + ogCtrlNodes = pathEditor.curveEditPoints.copy() + ogRotNodes = pathEditor.nodeRotations.copy() + + if not loadKeyframes: + return + + for i in range(len(ogNodes)): + frame = getPosKeyframeByIndex(i) + frame['position'] = ogNodes[i] + frame['rotation'] = ogRotNodes[i] + + + + def load(self): + global selFrame + global buttonMode + selFrame = -1 + buttonMode = False + + global ogNodes + global ogCtrlNodes + global ogRotNodes + + if len(ogNodes) != len(pathEditor.nodes): + + global keyFrames + + for i in range(len(keyFrames)-1,-1,-1): + if keyFrames[i]['type'] == 'position': + keyFrames.pop(i) + + self.updateNodes(False) + + for i in range(len(ogNodes)): + if len(ogNodes) == 1: + timeIndex = 0 + else: + timeIndex = round((i)/(len(ogNodes)-1) * (displayTicks-1)) + keyFrames.append({ + "type": "position", + "timeIndex": timeIndex, + "index": i, + "position": ogNodes[i], + "rotation": ogRotNodes[i] + }) + else: + self.updateNodes(True) + + self.refresh() + + + def unload(self): + pass \ No newline at end of file diff --git a/src/export.py b/src/export.py new file mode 100644 index 0000000..1285b1e --- /dev/null +++ b/src/export.py @@ -0,0 +1,252 @@ +import crossfiledialog +import struct +import copy + +pg = None +buttonEditor = None +events = [] + +#Save according to https://github.com/Team4388/2024AcrossTheRidgebotiverse/blob/Prep-For-Denver/src/main/java/frc4388/robot/commands/Autos/neo%20AutoRecoding%20format.txt + + +moveMultiplier = 0.1 +rotMultiplier = 1 + + +def buttonsToBytes(buttons): + data = 0 + for i in range(16): + data |= buttons[i] << i + return data.to_bytes(2, "little", signed=True) + + +def toByte(num): + if num > 255: + raise OverflowError + return num.to_bytes(1, 'big', signed=False) + + + +def toShort(num): + if num > 65535: + raise OverflowError + return num.to_bytes(2, 'big', signed=True) + + +def toInt(num): + return struct.pack('>i', num) + + +def toDouble(num): + return struct.pack('>d', num) + + + + +class xboxController: + def __init__(self): + self.buttons = [False for i in range(16)] + self.leftStick = (0,0) + self.rightStick = (0,0) + self.LT = -1 + self.RT = -1 + self.POV = -1 + + +def getPOVhat(up, down, left, right): + if up and right: + return 45 + elif right and down: + return 135 + elif down and left: + return 225 + elif left and up: + return 315 + elif up: + return 0 + elif right: + return 90 + elif down: + return 180 + elif left: + return 270 + else: + return -1 + + + + +def getSticksAtFrame(index): + + fractionIndex = round(index/buttonEditor.displayTickResolution) + + newpos, newrot = buttonEditor.getRobotAtIndex(fractionIndex) + oldpos, oldrot = buttonEditor.getRobotAtIndex(fractionIndex-1) + + # print(oldrot-newrot) + + diffPos = ((oldpos[0]-newpos[0])*moveMultiplier, (oldpos[1]-newpos[1])*moveMultiplier) + diffRot = (oldrot-newrot)*rotMultiplier + + if abs(diffPos[0]) > 1 or abs(diffPos[1]) > 1 or abs(diffRot) > 1: + print("Error! Robot moved too fast!, Try to edit 'Multiplier' values in export.py") + return (0, 0), 0 + + + print(diffPos) + + # print(diffRot) + + return diffPos, diffRot + + + +def getControllersAtFrame(index): + controllers = [xboxController(), xboxController()] + if index >= buttonEditor.matchTicks: + return controllers + + pos, rot = getSticksAtFrame(index) + + controllers[0].leftStick = pos + controllers[0].rightStick = (rot,0) + + btns = buttonEditor.getLeftButtonFrame(index) + if btns == None: + btns = buttonEditor.createBlankController() + else: + btns = btns['controllers'] + + for i in range(len(controllers)): + ctrlr = btns[i] + + controllers[i].buttons[0] = ctrlr['A'] + controllers[i].buttons[1] = ctrlr['B'] + controllers[i].buttons[2] = ctrlr['X'] + controllers[i].buttons[3] = ctrlr['Y'] + + controllers[i].buttons[4] = ctrlr['LB'] + controllers[i].buttons[5] = ctrlr['RB'] + + controllers[i].buttons[6] = ctrlr['Menu'] + controllers[i].buttons[7] = ctrlr['Windows'] + + controllers[i].buttons[8] = ctrlr['Left_Stick'] + controllers[i].buttons[9] = ctrlr['Right_Stick'] + + controllers[i].LT = (ctrlr['LT']*2)-1 + controllers[i].RT = (ctrlr['RT']*2)-1 + + controllers[i].POV = getPOVhat(ctrlr['Dpad_Up'], ctrlr['Dpad_Down'], + ctrlr['Dpad_Left'], ctrlr['Dpad_Right']) + + + + return controllers + + + +def getFrameData(index): + controllers = getControllersAtFrame(index) + data = b'' + for ctrlr in controllers: + # print(ctrlr.leftStick[0]) + data += toDouble(ctrlr.leftStick[0]) + data += toDouble(ctrlr.leftStick[1]) + data += toDouble(ctrlr.LT) + data += toDouble(ctrlr.RT) + data += toDouble(ctrlr.rightStick[0]) + data += toDouble(ctrlr.rightStick[1]) + + data += buttonsToBytes(ctrlr.buttons) + + # for btn in ctrlr.buttons: + # data += toBit(data) + # print(toBit(data)) + + data += toShort(ctrlr.POV) + + data += toInt(index * buttonEditor.tickTime) + + return data + + + +def getHeader(): + header = toByte(6) # Num Axes per controller + header += toByte(1) # Num POVs + header += toByte(2) # Num Controllers + header += toShort(buttonEditor.matchTicks) # Num Frames + return header + + + +def getData(): + data = b'' + for i in range(buttonEditor.matchTicks): + data += getFrameData(i) + return getHeader() + data + + + +def save(): + path = crossfiledialog.save_file('Save auto file', './') + # path = "./file.txt" + with open(path, "wb") as f: + f.write(getData()) + +class export: + name = "Export" + + def __init__(self, tmppg, tmprender, tmpbuttonEditor): + global pg + pg = tmppg + global render + render = tmprender + global buttonEditor + buttonEditor = tmpbuttonEditor + + self.loaded = False + + def getIsSelected(): + return False + + def getIsVisible(): + return self.loaded + + def onClick(pos): + save() + + render.addButton((round(render.width/4),round(render.screen.get_height()/4),round(render.width/2),round(render.height/2)), + "Export", getIsSelected, getIsVisible, onClick) + + def mouseDown(self, pos): + pass + + def mouseUp(self, pos): + pass + + def mouseMove(self, pos): + pass + + def doubleClick(self, pos): + pass + + def keyDown(self, key): + pass + + def load(self): + self.loaded = True + + # for keyFrame in buttonEditor.keyFrames: + # keyFrame['timeIndex'] = keyFrame['timeIndex'] * buttonEditor.displayTickResolution + + + render.clear() + pg.display.update() + + def unload(self): + self.loaded = False + + # for keyFrame in buttonEditor.keyFrames: + # keyFrame['timeIndex'] = round(keyFrame['timeIndex'] / buttonEditor.displayTickResolution) \ No newline at end of file diff --git a/src/pathEditor.py b/src/pathEditor.py new file mode 100644 index 0000000..fc43aec --- /dev/null +++ b/src/pathEditor.py @@ -0,0 +1,171 @@ +import math +from pygame.locals import * + +nodeColor = (255, 255, 255) +nodeRadius = 12 + +rotNodeDist = 35 +rotNodeColor = (255, 0, 255) +rotNodeRadius = 8 + +lineApproximationLineColor = (127, 127, 127, 0.5) +lineApproximationLineWidth = 3 + +curveEditPointColor = (0, 255, 255) +curveEditPointRadius = 8 + +curvePointCount = 80 + +nodes = [] +curveEditPoints = [] +nodeRotations = [] + +clickType = -1 +clickIndex = -1 + +render = None + + + + +def refresh(): + render.clear() + render.drawField() + + for i in range(0,len(curveEditPoints)): + render.line(lineApproximationLineColor, nodes[i], curveEditPoints[i], lineApproximationLineWidth) + render.line(lineApproximationLineColor, curveEditPoints[i], nodes[i+1], lineApproximationLineWidth) + + render.bezier(nodes[i], curveEditPoints[i], nodes[i+1], curvePointCount) + + render.circle(curveEditPointColor, curveEditPoints[i], curveEditPointRadius) + for i in range(0,len(nodeRotations)): + posX = (math.sin(nodeRotations[i])*rotNodeDist/render.offsetSize) + nodes[i][0] + posY = (math.cos(nodeRotations[i])*rotNodeDist/render.offsetSize) + nodes[i][1] + render.circle(rotNodeColor, (posX, posY), rotNodeRadius) + render.robotSquare(nodes[i], nodeRotations[i]) + for pos in nodes: + + render.circle(nodeColor, pos, nodeRadius) + render.update() + + + +def getElemAt(pos): + for i in range(0,len(nodes)): + if getDist(pos, nodes[i], nodeRadius): + return 0, i + for i in range(0,len(nodeRotations)): + posX = (math.sin(nodeRotations[i])*rotNodeDist/render.offsetSize) + nodes[i][0] + posY = (math.cos(nodeRotations[i])*rotNodeDist/render.offsetSize) + nodes[i][1] + if getDist(pos, (posX, posY), nodeRadius): + return 2, i + for i in range(0,len(curveEditPoints)): + if getDist(pos, curveEditPoints[i], curveEditPointRadius): + return 1, i + return -1, -1 + + + +def getDist(pos1, pos2, dist): + return math.sqrt(math.pow(pos1[0]-pos2[0], 2) + math.pow(pos1[1]-pos2[1], 2)) <= dist + + + +def addNode(pos): + nodes.append(pos) + if len(nodes) > 1: + index = len(nodes)-1 + # Middle point between current point and previous point + editPos = (nodes[index-1][0]+pos[0])/2,(nodes[index-1][1]+pos[1])/2 + curveEditPoints.append(editPos) + nodeRotations.append(nodeRotations[index-1]) + else: + nodeRotations.append(math.pi/2) + refresh() + + + +def nearestCirclePoint(center, pos, R): + vX = pos[0] - center[0] + vY = pos[1] - center[1] + magV = math.sqrt(vX*vX + vY*vY) + aX = center[0] + vX / magV * R + aY = center[1] + vY / magV * R + return (aX, aY) + + + +def points2rad(center, pos): + diffX = center[0] - pos[0] + diffY = center[1] - pos[1] + return -math.atan2(diffY, diffX) - (math.pi/2) + +class pathEditor: + name = "Path Editor" + + + def __init__(self, tmprender): + # global screen + # screen = tmpscreen + global render + render = tmprender + + refresh() + + + + def mouseDown(self, pos): + global clickType + global clickIndex + clickType, clickIndex = getElemAt(pos) + if clickType == -1: + addNode(pos) + + + + def mouseUp(self, pos): + global clickType + global clickIndex + if clickType != -1: + clickType = -1 + clickIndex = -1 + + + + def mouseMove(self, pos): + if clickType != -1: + if clickType == 0: + nodes[clickIndex] = pos + if clickType == 1: + curveEditPoints[clickIndex] = pos + if clickType == 2: + nodeRotations[clickIndex] = points2rad(nodes[clickIndex], nearestCirclePoint(nodes[clickIndex], pos, rotNodeDist/render.offsetSize)) + refresh() + + + + def doubleClick(self, pos): + clickType, clickIndex = getElemAt(pos) + if clickType == -1: + pass + elif clickType == 0: + if clickIndex > 0: + if clickIndex < len(nodes)-1: + newPos = (nodes[clickIndex-1][0]+nodes[clickIndex][0])/2,(nodes[clickIndex-1][1]+nodes[clickIndex][1])/2 + curveEditPoints[clickIndex] = newPos + curveEditPoints.pop(clickIndex-1) + elif clickIndex == 0 and len(nodes) > 1: + curveEditPoints.pop(clickIndex) + nodes.pop(clickIndex) + nodeRotations.pop(clickIndex) + refresh() + + def keyDown(self, key): + pass + + def load(self): + refresh() + + def unload(self): + pass \ No newline at end of file diff --git a/src/render.py b/src/render.py new file mode 100644 index 0000000..4b3d08a --- /dev/null +++ b/src/render.py @@ -0,0 +1,211 @@ +import math +import os +import sys +from pygame.locals import * +import numpy as np + +curvePointColor = (255, 255, 0) +curvePointRadius = 2 + +selTabBorderSize = 2 +selTabBorderIndent = 3 + +nodeSquareRadius = 35 +nodeSquareColor = (127, 127, 127, 0.5) +nodeSquareWidth = 3 + +nodeTickLength = 5 + +def image_path(relative_path): + try: + base_path = sys._MEIPASS + except Exception: + base_path = os.path.abspath(".") + + return os.path.join(base_path, relative_path) + + +class render(): + + + def __init__(self, pg, screen, topBarHeight, bottomBarHeight): + self.pg = pg + self.screen = screen + + self.topBarHeight = topBarHeight + self.bottomBarHeight = bottomBarHeight + + self.width = self.screen.get_width() + self.height = self.screen.get_width() * (643/1286) + self.rect = (0, self.topBarHeight, self.width, self.height+bottomBarHeight) + + self.font = self.pg.font.Font(None, 25) + + self.fieldImg = self.loadImg("images/Field.png") + self.offsetSize = self.fieldImg.get_width() / self.width + self.fieldImg = pg.transform.scale(self.fieldImg, (self.width, self.height)) + + self.elements = [] + + + + + def invert(self, img): + inv = self.pg.Surface(img.get_rect().size, self.pg.SRCALPHA) + inv.fill((255,255,255)) + inv.blit(img, (0,0), None, BLEND_RGB_SUB) + return inv + + + + def line(self, color, pos1, pos2, width): + self.pg.draw.line(self.screen, color, pos1, pos2, round(width/self.offsetSize)) + + + + def circle(self, color, pos, radius): + self.pg.draw.circle(self.screen, color, pos, radius/self.offsetSize) + + + + def drawrect(self, color, rect): + self.pg.draw.rect(self.screen, color, rect) + + + + # def drawText(self, text, color,): + + # text = self.font.render(text, True, color) + + # rect = text.get_rect() + + # self.screen.blit(text, rect) + # # text_rect = text.get_rect(center=(rect[0]+(rect[2]/2), rect[1]+(rect[3]/2))) + + + + def isInRect(self, pos, rect): + return pos[0] >= rect[0] and \ + pos[0] <= rect[0]+rect[2] and \ + pos[1] >= rect[1] and \ + pos[1] <= rect[1]+rect[3] + + + + def image(self, img, rect): + self.screen.blit(self.pg.transform.scale(img, (rect[2], rect[3])), rect) + + + + def loadImg(self, path): + return self.pg.image.load(image_path(path)).convert_alpha() + + + + def robotSquare(self, pos, rot): + pos1 = ((math.sin(rot + math.pi*-0.25)*nodeSquareRadius/self.offsetSize) + pos[0], + (math.cos(rot + math.pi*-0.25)*nodeSquareRadius/self.offsetSize) + pos[1]) + pos2 = ((math.sin(rot + math.pi*0.25)*nodeSquareRadius/self.offsetSize) + pos[0], + (math.cos(rot + math.pi*0.25)*nodeSquareRadius/self.offsetSize) + pos[1]) + pos3 = ((math.sin(rot + math.pi*0.75)*nodeSquareRadius/self.offsetSize) + pos[0], + (math.cos(rot + math.pi*0.75)*nodeSquareRadius/self.offsetSize) + pos[1]) + pos4 = ((math.sin(rot + math.pi*1.25)*nodeSquareRadius/self.offsetSize) + pos[0], + (math.cos(rot + math.pi*1.25)*nodeSquareRadius/self.offsetSize) + pos[1]) + + pos5 = ((math.sin(rot)*(nodeSquareRadius+nodeTickLength)/self.offsetSize) + pos[0], + (math.cos(rot)*(nodeSquareRadius+nodeTickLength)/self.offsetSize) + pos[1]) + pos6 = ((math.sin(rot)*(nodeSquareRadius-nodeTickLength)/self.offsetSize) + pos[0], + (math.cos(rot)*(nodeSquareRadius-nodeTickLength)/self.offsetSize) + pos[1]) + + self.line(nodeSquareColor, pos1, pos2, nodeSquareWidth*self.offsetSize) + self.line(nodeSquareColor, pos2, pos3, nodeSquareWidth*self.offsetSize) + self.line(nodeSquareColor, pos3, pos4, nodeSquareWidth*self.offsetSize) + self.line(nodeSquareColor, pos4, pos1, nodeSquareWidth*self.offsetSize) + + self.line(nodeSquareColor, pos5, pos6, nodeSquareWidth*self.offsetSize) + + + + + def bezier(self, p0, p1, p2, curvePointCount): + #for p in [p0, p1, p2]: + # pg.draw.circle(self.screen, (255, 255, 255), p, 5) + for t in np.arange(0, 1, 1/curvePointCount): + px = p0[0]*(1-t)**2 + 2*(1-t)*t*p1[0] + p2[0]*t**2 + py = p0[1]*(1-t)**2 + 2*(1-t)*t*p1[1] + p2[1]*t**2 + self.circle(curvePointColor, (px, py), curvePointRadius) + self.circle(curvePointColor, p2, curvePointRadius) + #self.drawrect(curvePointColor, (round(px+0.5), round(py+0.5), curvePointRadius, curvePointRadius)) + + + + def clear(self): + self.pg.draw.rect(self.screen, (0, 0, 0), self.rect) + + + + def drawField(self): + self.screen.blit(self.fieldImg, self.rect) + + + + def renderElements(self, pos): + for elem in self.elements: + if elem['type'] == 'button' and elem['getIsVisible'](): + # print(elem['getIsSelected']()) + self.renderButton(elem['rect'], elem['text'], elem['getIsSelected'](), pos) + + + + def clickElement(self, pos): + for elem in self.elements: + if elem['type'] == 'button' and elem['getIsVisible']() and self.isInRect(pos, elem['rect']): + elem['onClick'](pos) + + + + def update(self): + self.pg.display.update() + + + + def addButton(self, rect, text, getIsSelected, getIsVisible, onClick): + self.elements.append({ + "type": "button", + "text": text, + "getIsSelected": getIsSelected, + "getIsVisible": getIsVisible, + "onClick": onClick, + "rect": rect + }) + + + + def renderButton(self, rect, text, selected, mousePos): + + # print(isInRect(mousePos, rect)) + + if self.isInRect(mousePos, rect): + color = (16,64,32) + else: + color = (16,16,32) + + if selected: + borderColor = (0,255,0) + else: + borderColor = (64,127,127) + + text = self.font.render(text, True, (255,255,255)) + text_rect = text.get_rect(center=(rect[0]+(rect[2]/2), rect[1]+(rect[3]/2))) + + self.pg.draw.rect(self.screen, color, rect) + rect = (rect[0]+selTabBorderIndent,rect[1]+selTabBorderIndent, + rect[2]-selTabBorderIndent*2,rect[3]-selTabBorderIndent*2) + self.pg.draw.rect(self.screen, borderColor, rect) + rect = (rect[0]+selTabBorderSize,rect[1]+selTabBorderSize, + rect[2]-selTabBorderSize*2,rect[3]-selTabBorderSize*2) + self.pg.draw.rect(self.screen, color, rect) + + self.screen.blit(text, text_rect) + + self.update() \ No newline at end of file