Add high cube capabilities to 3rd robot
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I6ce3f4c1376bbb32d2326995e9b07675cb63ae4d
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.cc b/y2023_bot3/control_loops/superstructure/end_effector.cc
index 178e946..665aa9d 100644
--- a/y2023_bot3/control_loops/superstructure/end_effector.cc
+++ b/y2023_bot3/control_loops/superstructure/end_effector.cc
@@ -32,6 +32,8 @@
break;
case EndEffectorState::SPITTING_MID:
break;
+ case EndEffectorState::SPITTING_HIGH:
+ break;
}
}
@@ -43,6 +45,10 @@
state_ = EndEffectorState::SPITTING_MID;
}
+ if (roller_goal == RollerGoal::SPIT_HIGH) {
+ state_ = EndEffectorState::SPITTING_HIGH;
+ }
+
switch (state_) {
case EndEffectorState::IDLE:
// If idle and intake requested, intake
@@ -96,6 +102,14 @@
}
break;
+
+ case EndEffectorState::SPITTING_HIGH:
+ *roller_voltage = kRollerCubeSpitHighVoltage();
+
+ if (roller_goal == RollerGoal::IDLE) {
+ state_ = EndEffectorState::IDLE;
+ }
+ break;
}
}
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.h b/y2023_bot3/control_loops/superstructure/end_effector.h
index ab4e117..be0abc0 100644
--- a/y2023_bot3/control_loops/superstructure/end_effector.h
+++ b/y2023_bot3/control_loops/superstructure/end_effector.h
@@ -17,6 +17,7 @@
static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
static constexpr double kRollerCubeSpitMidVoltage() { return 5.0; }
+ static constexpr double kRollerCubeSpitHighVoltage() { return 6.37; }
EndEffector();
void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
index bc656fb..be7e646 100644
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
@@ -48,6 +48,14 @@
case PivotGoal::SCORE_MID_BACK:
pivot_goal = -0.763222056740831;
break;
+
+ case PivotGoal::SCORE_HIGH_FRONT:
+ pivot_goal = 0.846887672907125;
+ break;
+
+ case PivotGoal::SCORE_HIGH_BACK:
+ pivot_goal = -0.763222056740831;
+ break;
}
flatbuffers::Offset<
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
index 27a302c..d2b8e0b 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -10,6 +10,8 @@
SCORE_LOW_BACK = 4,
SCORE_MID_FRONT = 5,
SCORE_MID_BACK = 6,
+ SCORE_HIGH_FRONT = 7,
+ SCORE_HIGH_BACK = 8,
}
enum RollerGoal: ubyte {
@@ -17,6 +19,7 @@
INTAKE_CUBE = 1,
SPIT = 2,
SPIT_MID = 3,
+ SPIT_HIGH = 4,
}
table Goal {
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 7b2a3d2..10f8f6e 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -199,6 +199,14 @@
case PivotGoal::SCORE_MID_BACK:
pivot_goal = -0.763222056740831;
break;
+
+ case PivotGoal::SCORE_HIGH_FRONT:
+ pivot_goal = 0.846887672907125;
+ break;
+
+ case PivotGoal::SCORE_HIGH_BACK:
+ pivot_goal = -0.763222056740831;
+ break;
}
EXPECT_NEAR(pivot_goal,
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index 3d4947a..4492cb9 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -15,7 +15,10 @@
SPITTING = 3,
// Waiting for the arm to be at MID shooting goal and then telling the
// end effector to spit mid
- SPITTING_MID = 4
+ SPITTING_MID = 4,
+ // Waiting for the arm to be at HIGH shooting goal and then telling the
+ // end effector to spit mid
+ SPITTING_HIGH = 5
}
table Status {
diff --git a/y2023_bot3/joystick_reader.cc b/y2023_bot3/joystick_reader.cc
index f9f03b7..fd32652 100644
--- a/y2023_bot3/joystick_reader.cc
+++ b/y2023_bot3/joystick_reader.cc
@@ -95,6 +95,8 @@
if (data.IsPressed(kSpit)) {
roller_goal = RollerGoal::SPIT;
+ } else if (data.IsPressed(kSpitHigh)) {
+ roller_goal = RollerGoal::SPIT_HIGH;
}
if (data.IsPressed(kScore)) {