Copy

.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/

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)

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()

requirements.txt

@@ -0,0 +1,3 @@
+numpy
+pygame
+crossfiledialog

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

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)

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

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()