Spline UI: Make the robot the right size
Change-Id: I06b380c7408b99c93f9eb0df887077dab6d9cd29
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index cb0de5b..1a1ee96 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -7,9 +7,6 @@
#Set screen size for rest of program.
SCREEN_SIZE = screen.get_height() / 3
-WIDTH_OF_ROBOT = 0.65
-LENGTH_OF_ROBOT = 0.8
-
# Placeholder value
ROBOT_SIDE_TO_BALL_CENTER = 0.15
BALL_RADIUS = 0.165
@@ -19,7 +16,9 @@
HATCH_PANEL_WIDTH = 0.4826
FieldType = namedtuple(
- 'Field', ['name', 'tags', 'year', 'width', 'length', 'json_name'])
+ 'Field', ['name', 'tags', 'year', 'width', 'length', 'robot', 'json_name'])
+RobotType = namedtuple(
+ "Robot", ['width', 'length'])
GALACTIC_SEARCH = "Galactic Search"
ARED = "A Red"
@@ -31,6 +30,10 @@
SLALOM = "Slalom"
BARREL = "Barrel"
+Robot2019 = RobotType(width=0.65, length=0.8)
+Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
+Robot2021 = Robot2020
+
FIELDS = {
"2019 Field":
FieldType(
@@ -39,6 +42,7 @@
year=2019,
width=8.258302,
length=8.258302,
+ robot=Robot2019,
json_name="spline_2019.json"),
"2020 Field":
FieldType(
@@ -47,6 +51,7 @@
year=2020,
width=15.98295,
length=8.21055,
+ robot=Robot2020,
json_name="spline_2020.json"),
"2021 Galactic Search BRed":
FieldType(
@@ -55,6 +60,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="spline_red_b.json"),
"2021 Galactic Search ARed":
FieldType(
@@ -63,6 +69,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="spline_red_a.json"),
"2021 Galactic Search BBlue":
FieldType(
@@ -71,6 +78,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="spline_blue_b.json"),
"2021 Galactic Search ABlue":
FieldType(
@@ -79,6 +87,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="spline_blue_a.json"),
"2021 AutoNav Barrel":
FieldType(
@@ -87,6 +96,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="autonav_barrel.json"),
"2021 AutoNav Slalom":
FieldType(
@@ -95,6 +105,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="autonav_slalom.json"),
"2021 AutoNav Bounce":
FieldType(
@@ -103,6 +114,7 @@
year=2021,
width=9.144,
length=4.572,
+ robot=Robot2021,
json_name="autonav_bounce.json"),
}
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 2031416..3ecef40 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -118,8 +118,8 @@
distance = np.sqrt((p2[1] - p1[1])**2 + (p2[0] - p1[0])**2)
x_difference_o = p2[0] - p1[0]
y_difference_o = p2[1] - p1[1]
- x_difference = x_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
- y_difference = y_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+ x_difference = x_difference_o * mToPx(FIELD.robot.length / 2) / distance
+ y_difference = y_difference_o * mToPx(FIELD.robot.length / 2) / distance
front_middle = []
front_middle.append(p1[0] + x_difference)
@@ -132,8 +132,8 @@
slope = [-(1 / x_difference_o) / (1 / y_difference_o)]
angle = np.arctan(slope)
- x_difference = np.sin(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
- y_difference = np.cos(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+ x_difference = np.sin(angle[0]) * mToPx(FIELD.robot.width / 2)
+ y_difference = np.cos(angle[0]) * mToPx(FIELD.robot.width / 2)
front_1 = []
front_1.append(front_middle[0] - x_difference)
@@ -152,9 +152,9 @@
back_2.append(back_middle[1] + y_difference)
x_difference = x_difference_o * mToPx(
- LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+ FIELD.robot.length / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
y_difference = y_difference_o * mToPx(
- LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+ FIELD.robot.length / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
#Calculate Ball
ball_center = []
@@ -162,9 +162,9 @@
ball_center.append(p1[1] + y_difference)
x_difference = x_difference_o * mToPx(
- LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+ FIELD.robot.length / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
y_difference = y_difference_o * mToPx(
- LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+ FIELD.robot.length / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
#Calculate Panel
panel_center = []