Merge "Make AutoNav Autonomouses"
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index d4306ac..03476d9 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -131,10 +131,17 @@
},
SplineDirection::kForward);
+ set_intake_goal(1.2);
+ set_roller_voltage(7.0);
+ SendSuperstructureGoal();
+
if (!spline1.WaitForPlan()) return;
spline1.Start();
if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+ set_intake_goal(-0.89);
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
}
}
@@ -275,5 +282,40 @@
StartDrive(1.0, 0.0, kDrive, kTurn);
return WaitForDriveDone();
}
+
+void AutonomousActor::SendSuperstructureGoal() {
+
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ frc971::ProfileParameters::Builder profile_params_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+ profile_params_builder.add_max_velocity(0.0);
+ profile_params_builder.add_max_acceleration(0.0);
+ flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
+ profile_params_builder.Finish();
+ intake_builder.add_unsafe_goal(intake_goal_);
+ intake_builder.add_profile_params(profile_params_offset);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_intake(intake_offset);
+ superstructure_builder.add_roller_voltage(roller_voltage_);
+ superstructure_builder.add_roller_speed_compensation(kRollerSpeedCompensation);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+
+}
} // namespace actors
} // namespace y2020
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 79bcce5..f6ef138 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -9,10 +9,16 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/actors/auto_splines.h"
#include "y2020/vision/galactic_search_path_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
namespace y2020 {
namespace actors {
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+namespace superstructure = y2020::control_loops::superstructure;
+
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
@@ -22,6 +28,13 @@
private:
void Reset();
+
+ void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
+ void set_roller_voltage(double roller_voltage) {
+ roller_voltage_ = roller_voltage;
+ }
+
+ void SendSuperstructureGoal();
void SplineAuto();
void SendStartingPosition(double x, double y, double theta);
void SendStartingPosition(const frc971::MultiSpline *const spline);
@@ -31,8 +44,14 @@
void AutoNavSlalom();
bool DriveFwd();
+ double intake_goal_ = 0.0;
+ double roller_voltage_ = 0.0;
+ const float kRollerSpeedCompensation = 2.0;
+
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
+ ::aos::Sender<::y2020::control_loops::superstructure::Goal>
+ superstructure_goal_sender_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<y2020::vision::GalacticSearchPath> path_fetcher_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
diff --git a/y2020/vision/ball_detection.py b/y2020/vision/ball_detection.py
index 46faccc..7f2dd6f 100644
--- a/y2020/vision/ball_detection.py
+++ b/y2020/vision/ball_detection.py
@@ -4,7 +4,6 @@
import cv2 as cv
import numpy as np
-
# This function finds the percentage of yellow pixels in the rectangles
# given that are regions of the given image. This allows us to determine
# whether there is a ball in those rectangles
@@ -27,5 +26,4 @@
video_stream = cv.VideoCapture(0)
frame = video_stream.read()[1]
video_stream.release()
- frame = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
return frame
diff --git a/y2020/vision/galactic_search_path.py b/y2020/vision/galactic_search_path.py
index a4acb05..242253f 100644
--- a/y2020/vision/galactic_search_path.py
+++ b/y2020/vision/galactic_search_path.py
@@ -1,14 +1,15 @@
#!/usr/bin/python3
-from rect import Rect
-import ball_detection
-
# Creates a UI for a user to select the regions in a camera image where the balls could be placed.
# After the balls have been placed on the field and they submit the regions,
# it will take another picture and based on the yellow regions in that picture it will determine where the
# balls are. This tells us which path the current field is. It then sends the Alliance and Letter of the path
# with aos_send to the /camera channel for the robot to excecute the spline for that path.
+from rect import Rect
+import ball_detection
+
+import cv2 as cv
from enum import Enum
import glog
import json
@@ -30,15 +31,14 @@
NUM_RECTS = 4
AOS_SEND_PATH = "bazel-bin/aos/aos_send"
-CONFIG_PATH = "bazel-bin/y2020/config.json"
if os.path.isdir("/home/pi/robot_code"):
AOS_SEND_PATH = "/home/pi/robot_code/aos_send.stripped"
- CONFIG_PATH = "/home/pi/robot_code/config.stripped.json"
+ os.system("./starter_cmd stop camera_reader")
# The minimum percentage of yellow for a region of a image to
# be considered to have a ball
-BALL_PCT_THRESHOLD = 50
+BALL_PCT_THRESHOLD = 10
rects = [Rect(None, None, None, None)]
@@ -87,9 +87,25 @@
rects[rect_index].y2 = None
plt.show()
+SLEEP = 100
+img_fig = None
+
def on_submit(event):
+ global img_fig
plt.close("all")
- pcts = ball_detection.pct_yellow(ball_detection.capture_img(), rects)
+ plt.ion()
+ img_fig = plt.figure()
+ running = True
+ while running:
+ detect_path()
+ cv.waitKey(SLEEP)
+
+def detect_path():
+ img = ball_detection.capture_img()
+ img_fig.figimage(img)
+ plt.show()
+ plt.pause(0.001)
+ pcts = ball_detection.pct_yellow(img, rects)
if len(pcts) == len(rects):
paths = []
for i in range(len(pcts)):
@@ -106,8 +122,8 @@
glog.warn("More than one ball found, path is unknown" if rects_with_balls > 1 else
"No balls found")
glog.info("Path is %s" % path)
- os.system(AOS_SEND_PATH + " --config " + CONFIG_PATH +
- "/pi1/camera y2020.vision.GalacticSearchPath '" + json.dumps(path) + "'")
+ os.system(AOS_SEND_PATH +
+ " /pi2/camera y2020.vision.GalacticSearchPath '" + json.dumps(path) + "'")
for j in range(len(pcts)):
glog.info("%s: %s%% yellow" % (rects[j], pcts[j]))