ChargedUp2023/generate_path.py at main · smthae/ChargedUp2023 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
import json
import os

FIELD_LENGTH = 16.54175
FIELD_WIDTH = 8.0137
DIRECTORY = "./src/main/deploy/pathplanner"


def generate_red_aliance(file_name: str, directory: str):
    waypoints = []

    path_file = open(f"{directory}/{file_name}", 'r', encoding="utf-8")
    new_path_file = open(
        f"{directory}/{file_name.split('.')[0]}_red.path", "w", encoding="utf-8")

    path_data = json.loads(path_file.read())

    for waypoint in path_data["waypoints"]:
        waypoint["anchorPoint"]["x"] = FIELD_LENGTH - \
            waypoint["anchorPoint"]["x"]
        if 180 - waypoint["holonomicAngle"] == 0:
            waypoint["holonomicAngle"] = -0.01
        else:
            waypoint["holonomicAngle"] = 180 - waypoint["holonomicAngle"]

        if waypoint["nextControl"] != None:
            waypoint["nextControl"]["x"] = FIELD_LENGTH - \
                waypoint["nextControl"]["x"]

        if waypoint["prevControl"] != None:
            waypoint["prevControl"]["x"] = FIELD_LENGTH - \
                waypoint["prevControl"]["x"]

        waypoints.append(waypoint)

    path_data["waypoints"] = waypoints
    new_path_file.write(json.dumps(path_data))

    path_file.close()
    new_path_file.close()


generate_red_aliance("cableside.path", DIRECTORY)