Merge "Changed profiled subsystem dt."
diff --git a/aos/BUILD b/aos/BUILD
index 47de872..2bd49c1 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -5,7 +5,6 @@
name = "prime_binaries",
srcs = [
"//aos:aos_dump",
- "//aos:core",
"//aos/starter",
],
visibility = ["//visibility:public"],
@@ -23,7 +22,6 @@
name = "prime_binaries_stripped",
srcs = [
# starter is hard coded to look for a non-stripped core...
- "//aos:core",
"//aos:aos_dump.stripped",
"//aos/starter",
],
diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index 22db82a..83a39c8 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -101,7 +101,7 @@
char test_device[10];
for (char i = 'a'; i < 'z'; ++i) {
snprintf(test_device, sizeof(test_device), "/dev/sd%c", i);
- LOG(INFO) << "Trying to access" << test_device;
+ VLOG(1) << "Trying to access" << test_device;
if (access(test_device, F_OK) != -1) {
snprintf(device, device_size, "sd%c", i);
return true;
diff --git a/aos/network/message_bridge_client.cc b/aos/network/message_bridge_client.cc
index c44eee0..891a8a1 100644
--- a/aos/network/message_bridge_client.cc
+++ b/aos/network/message_bridge_client.cc
@@ -3,7 +3,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-DEFINE_string(config, "multinode_pingpong_config.json", "Path to the config.");
+DEFINE_string(config, "config.json", "Path to the config.");
namespace aos {
namespace message_bridge {
diff --git a/aos/network/message_bridge_server.cc b/aos/network/message_bridge_server.cc
index fa5e7c1..ee276d6 100644
--- a/aos/network/message_bridge_server.cc
+++ b/aos/network/message_bridge_server.cc
@@ -4,7 +4,7 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
-DEFINE_string(config, "multinode_pingpong_config.json", "Path to the config.");
+DEFINE_string(config, "config.json", "Path to the config.");
namespace aos {
namespace message_bridge {
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index f54e2bf..53afe84 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -227,17 +227,24 @@
server_connection_.resize(event_loop->configuration()->nodes()->size());
// Seed up all the per-node connection state.
+ // We are making the assumption here that every connection is bidirectional
+ // (data is being sent both ways). This is pretty safe because we are
+ // forwarding timestamps between nodes.
for (std::string_view destination_node_name :
configuration::DestinationNodeNames(event_loop->configuration(),
event_loop->node())) {
// Find the largest connection message so we can size our buffers big enough
- // to receive a connection message.
- max_size = std::max(
- max_size,
- static_cast<int32_t>(MakeConnectMessage(event_loop->configuration(),
- event_loop->node(),
- destination_node_name)
- .size()));
+ // to receive a connection message. The connect message comes from the
+ // client to the server, so swap the node arguments.
+ const int32_t connect_size = static_cast<int32_t>(
+ MakeConnectMessage(event_loop->configuration(),
+ configuration::GetNode(event_loop->configuration(),
+ destination_node_name),
+ event_loop->node()->name()->string_view())
+ .size());
+ VLOG(1) << "Connection to " << destination_node_name << " has size "
+ << connect_size;
+ max_size = std::max(max_size, connect_size);
const Node *destination_node = configuration::GetNode(
event_loop->configuration(), destination_node_name);
@@ -315,6 +322,7 @@
}
// Buffer up the max size a bit so everything fits nicely.
+ LOG(INFO) << "Max message size for all clients is " << max_size;
server_.SetMaxSize(max_size + 100);
statistics_timer_ = event_loop_->AddTimer([this]() { Tick(); });
diff --git a/aos/starter/starter.cc b/aos/starter/starter.cc
index 6860cc4..5d78b5a 100644
--- a/aos/starter/starter.cc
+++ b/aos/starter/starter.cc
@@ -73,7 +73,7 @@
void operator()(event *evt) {
if (evt == NULL) return;
if (event_del(evt) != 0) {
- AOS_LOG(WARNING, "event_del(%p) failed\n", evt);
+ LOG(WARNING) << "event_del(" << evt << ") failed";
}
}
};
@@ -140,16 +140,16 @@
void RemoveWatchFromMap() {
int watch = watch_to_remove_;
if (watch == -1) {
- AOS_CHECK_NE(watch_, -1);
+ CHECK_NE(watch_, -1);
watch = watch_;
}
if (watchers[watch] != this) {
- AOS_LOG(WARNING, "watcher for %s (%p) didn't find itself in the map\n",
- filename_.c_str(), this);
+ LOG(WARNING) << "watcher for " << filename_ << " (" << this
+ << ") didn't find itself in the map";
} else {
watchers.erase(watch);
}
- AOS_LOG(DEBUG, "removed watch ID %d\n", watch);
+ VLOG(1) << "removed watch ID " << watch;
if (watch_to_remove_ == -1) {
watch_ = -1;
} else {
@@ -158,20 +158,19 @@
}
void CreateWatch() {
- AOS_CHECK_EQ(watch_, -1);
+ CHECK_EQ(watch_, -1);
watch_ = inotify_add_watch(notify_fd, filename_.c_str(),
create_ ? IN_CREATE : (IN_ATTRIB |
IN_MODIFY |
IN_DELETE_SELF |
IN_MOVE_SELF));
if (watch_ == -1) {
- AOS_PLOG(FATAL,
- "inotify_add_watch(%d, %s,"
- " %s ? IN_CREATE : (IN_ATTRIB | IN_MODIFY)) failed",
- notify_fd, filename_.c_str(), create_ ? "true" : "false");
+ PLOG(FATAL) << "inotify_add_watch(" << notify_fd << ", " << filename_
+ << ", " << (create_ ? "true" : "false")
+ << " ? IN_CREATE : (IN_ATTRIB | IN_MODIFY)) failed";
}
watchers[watch_] = this;
- AOS_LOG(DEBUG, "watch for %s is %d\n", filename_.c_str(), watch_);
+ VLOG(1) << "watch for " << filename_ << " is " << watch_;
}
// This gets set up as the callback for EV_READ on the inotify file
@@ -180,7 +179,7 @@
unsigned int to_read;
// Use FIONREAD to figure out how many bytes there are to read.
if (ioctl(notify_fd, FIONREAD, &to_read) < 0) {
- AOS_PLOG(FATAL, "FIONREAD(%d, %p) failed", notify_fd, &to_read);
+ PLOG(FATAL) << "FIONREAD(" << notify_fd << ", " << &to_read << ") failed";
}
inotify_event *notifyevt = static_cast<inotify_event *>(malloc(to_read));
const char *end = reinterpret_cast<char *>(notifyevt) + to_read;
@@ -191,8 +190,8 @@
AOS_PLOG(FATAL, "read(%d, %p, %u) failed", notify_fd, notifyevt, to_read);
}
if (static_cast<size_t>(ret) != to_read) {
- AOS_LOG(ERROR, "read(%d, %p, %u) returned %zd instead of %u\n", notify_fd,
- notifyevt, to_read, ret, to_read);
+ LOG(ERROR) << "read(" << notify_fd << ", " << notifyevt << ", " << to_read
+ << ") returned " << ret << " instead of " << to_read;
return;
}
@@ -200,9 +199,10 @@
// multiple events at once.
while (reinterpret_cast<char *>(notifyevt) < end) {
if (watchers.count(notifyevt->wd) != 1) {
- AOS_LOG(WARNING, "couldn't find whose watch ID %d is\n", notifyevt->wd);
+ LOG(WARNING) << "couldn't find whose watch ID " << notifyevt->wd
+ << " is";
} else {
- AOS_LOG(DEBUG, "mask=%" PRIu32 "\n", notifyevt->mask);
+ VLOG(1) << "mask=" << notifyevt->mask;
// If the watch was removed.
if (notifyevt->mask & IN_IGNORED) {
watchers[notifyevt->wd]->WatchDeleted();
@@ -222,7 +222,7 @@
// INotifyReadable calls this method whenever the watch for our file gets
// removed somehow.
void WatchDeleted() {
- AOS_LOG(DEBUG, "watch for %s deleted\n", filename_.c_str());
+ VLOG(1) << "watch for " << filename_ << " deleted";
RemoveWatchFromMap();
CreateWatch();
}
@@ -230,7 +230,7 @@
// INotifyReadable calls this method whenever the watch for our file triggers.
void FileNotified(const char *filename) {
AOS_CHECK_NE(watch_, -1);
- AOS_LOG(DEBUG, "got a notification for %s\n", filename_.c_str());
+ VLOG(1) << "got a notification for " << filename_;
if (!check_filename_.empty()) {
if (filename == NULL) {
@@ -315,8 +315,7 @@
}
if (feof(pipe)) {
- AOS_LOG(FATAL, "`%s` failed. didn't print a whole line\n",
- command.c_str());
+ LOG(FATAL) << "`" << command << "` failed. didn't print a whole line";
}
}
@@ -329,7 +328,7 @@
}
if (child_status != 0) {
- AOS_LOG(FATAL, "`%s` failed. return %d\n", command.c_str(), child_status);
+ LOG(FATAL) << "`" << command << "` failed. return " << child_status;
}
return std::string(result.get());
@@ -349,16 +348,14 @@
time_timeval.tv_usec = usec.count();
}
if (evtimer_add(timeout.release(), &time_timeval) != 0) {
- AOS_LOG(FATAL, "evtimer_add(%p, %p) failed\n", timeout.release(),
- &time_timeval);
+ LOG(FATAL) << "evtimer_add(" << timeout.release() << ", " << &time_timeval
+ << ") failed";
}
}
class Child;
-// This is where all of the Child instances except core live.
+// This is where all of the Child instances live.
std::vector<unique_ptr<Child>> children;
-// A global place to hold on to which child is core.
-unique_ptr<Child> core;
// Represents a child process. It will take care of restarting itself etc.
class Child {
@@ -401,7 +398,7 @@
monotonic_clock::time_point oldest = restarts_.front();
restarts_.pop();
if (monotonic_clock::now() <= kMaxRestartsTime + oldest) {
- AOS_LOG(WARNING, "process %s getting restarted too often\n", name());
+ LOG(WARNING) << "process " << name() << " getting restarted too often";
Timeout(kResumeWait, StaticStart, this);
return;
}
@@ -441,7 +438,7 @@
}
void FileModified() {
- AOS_LOG(DEBUG, "file for %s modified\n", name());
+ LOG(INFO) << "file for " << name() << " modified";
struct timeval restart_time_timeval;
{
::std::chrono::seconds sec =
@@ -455,18 +452,14 @@
}
// This will reset the timeout again if it hasn't run yet.
if (evtimer_add(restart_timeout.get(), &restart_time_timeval) != 0) {
- AOS_LOG(FATAL, "evtimer_add(%p, %p) failed\n", restart_timeout.get(),
- &restart_time_timeval);
+ LOG(FATAL) << "evtimer_add(" << restart_timeout.get() << ", "
+ << &restart_time_timeval << ") failed";
}
waiting_to_restart.insert(this);
}
static void StaticDoRestart(int, short, void *) {
- AOS_LOG(DEBUG, "restarting everything that needs it\n");
- if (waiting_to_restart.find(core.get()) != waiting_to_restart.end()) {
- core->DoRestart();
- waiting_to_restart.erase(core.get());
- }
+ LOG(INFO) << "restarting everything that needs it";
for (auto c : waiting_to_restart) {
c->DoRestart();
}
@@ -483,18 +476,14 @@
¤t_stat);
}
if (current_stat.st_mtime == stat_at_start_.st_mtime) {
- AOS_LOG(DEBUG, "ignoring trigger for %s because mtime didn't change\n",
- name());
+ LOG(INFO) << "ignoring trigger for " << name()
+ << " because mtime didn't change";
return;
}
}
- if (this == core.get()) {
- fprintf(stderr, "Restarting core -> exiting now.\n");
- exit(0);
- }
if (pid_ != -1) {
- AOS_LOG(DEBUG, "sending SIGTERM to child %d to restart it\n", pid_);
+ LOG(INFO) << "sending SIGTERM to child " << pid_ << " to restart it";
if (kill(pid_, SIGTERM) == -1) {
AOS_PLOG(WARNING, "kill(%d, SIGTERM) failed", pid_);
}
@@ -503,7 +492,7 @@
status->old_pid = pid_;
Timeout(kProcessDieTime, StaticCheckDied, status);
} else {
- AOS_LOG(WARNING, "%s restart attempted but not running\n", name());
+ LOG(WARNING) << name() << " restart attempted but not running";
}
}
@@ -516,9 +505,9 @@
// Checks to see if the child using the PID old_pid is still running.
void CheckDied(pid_t old_pid) {
if (pid_ == old_pid) {
- AOS_LOG(WARNING, "child %d refused to die\n", old_pid);
+ LOG(WARNING) << "child " << old_pid << " refused to die";
if (kill(old_pid, SIGKILL) == -1) {
- AOS_PLOG(WARNING, "kill(%d, SIGKILL) failed", old_pid);
+ LOG(WARNING) << "kill(" << old_pid << ", SIGKILL) failed";
}
}
}
@@ -530,8 +519,8 @@
// Actually starts the child.
void Start() {
if (pid_ != -1) {
- AOS_LOG(WARNING, "calling Start() but already have child %d running\n",
- pid_);
+ LOG(WARNING) << "calling Start() but already have child " << pid_
+ << " running";
if (kill(pid_, SIGKILL) == -1) {
AOS_PLOG(WARNING, "kill(%d, SIGKILL) failed", pid_);
return;
@@ -571,7 +560,7 @@
if (pid_ == -1) {
AOS_PLOG(FATAL, "forking to run \"%s\" failed", binary_.c_str());
}
- AOS_LOG(DEBUG, "started \"%s\" successfully\n", binary_.c_str());
+ LOG(INFO) << "started \"" << binary_ << "\" successfully";
}
// A history of the times that this process has been restarted.
@@ -658,10 +647,6 @@
}
}
- if (pid == core->pid()) {
- return core;
- }
-
static const unique_ptr<Child> kNothing;
return kNothing;
}
@@ -688,44 +673,40 @@
if (child) {
switch (infop.si_code) {
case CLD_EXITED:
- AOS_LOG(WARNING, "child %d (%s) exited with status %d\n", pid,
- child->name(), status);
+ LOG(WARNING) << "child " << pid << " (" << child->name()
+ << ") exited with status " << status;
break;
case CLD_DUMPED:
- AOS_LOG(INFO,
- "child %d actually dumped core. "
- "falling through to killed by signal case\n",
- pid);
+ LOG(INFO) << "child " << pid
+ << " actually dumped core. falling through to killed by "
+ "signal case";
[[fallthrough]];
/* FALLTHRU */
case CLD_KILLED:
// If somebody (possibly us) sent it SIGTERM that means that they just
// want it to stop, so it stopping isn't a WARNING.
- AOS_LOG((status == SIGTERM) ? DEBUG : WARNING,
- "child %d (%s) was killed by signal %d (%s)\n", pid,
- child->name(), status, aos_strsignal(status));
+ ((status == SIGTERM) ? LOG(INFO) : LOG(WARNING))
+ << "child " << pid << " (" << child->name()
+ << ") was killed by signal " << status << " ("
+ << aos_strsignal(status) << ")";
break;
case CLD_STOPPED:
- AOS_LOG(WARNING,
- "child %d (%s) was stopped by signal %d "
- "(giving it a SIGCONT(%d))\n",
- pid, child->name(), status, SIGCONT);
+ LOG(WARNING) << "child " << pid << " (" << child->name()
+ << ") was stopped by signal " << status
+ << " (giving it a SIGCONT(" << SIGCONT << "))";
kill(pid, SIGCONT);
continue;
default:
- AOS_LOG(WARNING, "something happened to child %d (%s) (killing it)\n",
- pid, child->name());
+ LOG(WARNING) << "something happened to child " << pid << " ("
+ << child->name() << ") (killing it)";
kill(pid, SIGKILL);
continue;
}
} else {
- AOS_LOG(WARNING, "couldn't find a Child for pid %d\n", pid);
+ LOG(WARNING) << "couldn't find a Child for pid " << pid;
return;
}
- if (child == core) {
- AOS_LOG(FATAL, "core died\n");
- }
child->ProcessDied();
}
}
@@ -734,7 +715,7 @@
// start from main to Run.
const char *child_list_file;
-void Run(void *watch);
+void Run();
void Main() {
logging::Init();
@@ -777,44 +758,15 @@
libevent_base = EventBaseUniquePtr(event_base_new());
- std::string core_touch_file = "/tmp/starter.";
- core_touch_file += std::to_string(static_cast<intmax_t>(getpid()));
- core_touch_file += ".core_touch_file";
- const int result =
- ::aos::util::RunCommand(("touch '" + core_touch_file + "'").c_str());
- if (result == -1) {
- AOS_PLOG(FATAL, "running `touch '%s'` failed\n", core_touch_file.c_str());
- } else if (!WIFEXITED(result) || WEXITSTATUS(result) != 0) {
- AOS_LOG(FATAL, "`touch '%s'` gave result %x\n", core_touch_file.c_str(),
- result);
- }
- FileWatch core_touch_file_watch(core_touch_file, Run, NULL);
- core = unique_ptr<Child>(
- new Child("core " + core_touch_file));
-
- FILE *pid_file = fopen("/tmp/starter.pid", "w");
- if (pid_file == NULL) {
- AOS_PLOG(FATAL, "fopen(\"/tmp/starter.pid\", \"w\") failed");
- } else {
- if (fprintf(pid_file, "%d", core->pid()) == -1) {
- AOS_PLOG(WARNING, "fprintf(%p, \"%%d\", %d) failed", pid_file,
- core->pid());
- }
- fclose(pid_file);
- }
-
- AOS_LOG(INFO, "waiting for %s to appear\n", core_touch_file.c_str());
+ Run();
event_base_dispatch(libevent_base.get());
- AOS_LOG(FATAL, "event_base_dispatch(%p) returned\n", libevent_base.get());
+ LOG(FATAL) << "event_base_dispatch(" << libevent_base.get() << ") returned";
}
// This is the callback for when core creates the file indicating that it has
// started.
-void Run(void *watch) {
- // Make it so it doesn't keep on seeing random changes in /tmp.
- static_cast<FileWatch *>(watch)->RemoveWatch();
-
+void Run() {
// It's safe now because core is up.
aos::InitNRT();
@@ -827,7 +779,7 @@
break;
}
if (list_file.rdstate() != 0) {
- AOS_LOG(FATAL, "reading input file %s failed\n", child_list_file);
+ LOG(FATAL) << "reading input file " << child_list_file << " failed";
}
children.push_back(unique_ptr<Child>(new Child(child_name)));
}
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index f0b9537..5ad175e 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -161,7 +161,7 @@
// This assumes that res is created in python to be getPathLength() long.
// Likely to SEGFAULT otherwise.
- void Distances(Trajectory *t, double *res) {
+ void TrajectoryDistances(Trajectory *t, double *res) {
const ::std::vector<double> &distances = t->Distances();
::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
}
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 50764c7..1fae02b 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -164,18 +164,25 @@
)
py_binary(
- name = "path_edit",
+ name = "spline_graph",
srcs = [
- "basic_window.py",
"color.py",
+ "spline_drawing.py",
+ "spline_writer.py",
"path_edit.py",
+ "points.py",
+ "graph.py",
+ "spline_graph.py",
],
+ legacy_create_init = False,
restricted_to = ["//tools:k8"],
visibility = ["//visibility:public"],
deps = [
":libspline",
":python_init",
"@python_gtk",
+ "@matplotlib_repo//:matplotlib2.7",
+ ":basic_window",
],
)
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
old mode 100644
new mode 100755
index 78324a3..827fe64
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -1,3 +1,4 @@
+#!/usr/bin/python3
import gi
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
@@ -5,10 +6,10 @@
from gi.repository import Gdk
from gi.repository import GdkX11
import cairo
+from constants import *
identity = cairo.Matrix()
-
# Override the matrix of a cairo context.
class OverrideMatrix(object):
def __init__(self, cr, matrix):
@@ -34,7 +35,6 @@
def quit_main_loop(*args):
mainloop.quit()
-
def RunApp():
try:
mainloop.run()
@@ -49,11 +49,10 @@
# Draw in response to an expose-event
def __init__(self):
super(BaseWindow, self).__init__()
- #self.window.connect("destroy", quit_main_loop)
- self.set_size_request(640, 600)
+ self.set_size_request(2*SCREEN_SIZE, SCREEN_SIZE)
self.center = (0, 0)
- self.shape = (640, 400)
+ self.shape = (2*SCREEN_SIZE, SCREEN_SIZE)
self.needs_redraw = False
def get_current_scale(self):
@@ -81,3 +80,4 @@
# Handle the expose-event by drawing
def handle_draw(self, cr):
pass
+
diff --git a/frc971/control_loops/python/color.py b/frc971/control_loops/python/color.py
index ec53e82..5634042 100644
--- a/frc971/control_loops/python/color.py
+++ b/frc971/control_loops/python/color.py
@@ -1,9 +1,10 @@
class Color:
def __init__(self, r, g, b, a=1.0):
- self.r = r
- self.g = g
- self.b = b
- self.a = a
+ self.r = r
+ self.g = g
+ self.b = b
+ self.a = a
+
palette = {
"RED": Color(1, 0, 0),
@@ -16,5 +17,6 @@
"WHITE": Color(1, 1, 1),
"GREY": Color(0.5, 0.5, 0.5),
"LIGHT_GREY": Color(0.75, 0.75, 0.75),
- "DARK_GREY": Color(0.25, 0.25, 0.25)
+ "DARK_GREY": Color(0.25, 0.25, 0.25),
+ "ORANGE": Color(1, 0.65, 0)
}
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
new file mode 100644
index 0000000..7b45215
--- /dev/null
+++ b/frc971/control_loops/python/constants.py
@@ -0,0 +1,28 @@
+import argparse
+
+arg_parser = argparse.ArgumentParser(description='spline_editor')
+arg_parser.add_argument(
+ 'size',
+ metavar='N',
+ default=800,
+ type=int,
+ nargs='?',
+ help="size of the screen")
+args = arg_parser.parse_args()
+SCREEN_SIZE = args.size
+
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+
+WIDTH_OF_ROBOT = 0.65
+LENGTH_OF_ROBOT = 0.8
+
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 #Placeholder value
+BALL_RADIUS = 0.165
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 #Placeholder value
+HATCH_PANEL_WIDTH = 0.4826
+
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
+
+def mToPx(m):
+ return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
new file mode 100644
index 0000000..89979f0
--- /dev/null
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -0,0 +1,314 @@
+import cairo
+from color import Color, palette
+from constants import *
+import numpy as np
+
+
+def set_color(cr, color, a=1):
+ if color.a == 1.0:
+ cr.set_source_rgba(color.r, color.g, color.b, a)
+ else:
+ cr.set_source_rgba(color.r, color.g, color.b, color.a)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+ """Draws a cross with fixed dimensions in pixel space."""
+ set_color(cr, color)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+ set_color(cr, palette["WHITE"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ set_color(cr, color)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+ set_color(cr, palette["WHITE"])
+
+
+def draw_control_points(cr, points, width=10, radius=4, color=palette["BLUE"]):
+ for i in range(0, len(points)):
+ draw_px_x(cr, points[i][0], points[i][1], width, color)
+ set_color(cr, color)
+ cr.arc(points[i][0], points[i][1], radius, 0, 2.0 * np.pi)
+ cr.fill()
+ set_color(cr, palette["WHITE"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+ cr.scale(widtha, -heighta)
+ cr.show_text(text)
+ cr.scale(widthb, -heightb)
+
+
+def draw_HAB(cr):
+ # BASE Constants
+ X_BASE = 0
+ Y_BASE = 0
+ R = 0.381 - .1
+ BACKWALL_X = X_BASE
+ LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+ # HAB Levels 2 and 3 called in variables backhab
+ # draw loading stations
+ cr.move_to(0, LOADING_Y)
+ cr.line_to(mToPx(0.6), LOADING_Y)
+ cr.move_to(mToPx(R), LOADING_Y)
+ cr.arc(mToPx(R), LOADING_Y, 5, 0, np.pi * 2.0)
+ cr.move_to(0, -1.0 * LOADING_Y)
+ cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+ cr.move_to(mToPx(R), -1.0 * LOADING_Y)
+ cr.arc(mToPx(R), -1.0 * LOADING_Y, 5, 0, np.pi * 2.0)
+
+ # HAB Levels 2 and 3 called in variables backhab
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ #draw 0, 0
+ set_color(cr, palette["BLACK"])
+ cr.move_to(0, 0)
+ cr.line_to(0, 0 + mToPx(8.2296 / 2.0))
+ cr.move_to(0, 0)
+ cr.line_to(0, 0 + mToPx(-8.2296 / 2.0))
+ cr.move_to(0, 0)
+ cr.line_to(0 + mToPx(8.2296), 0)
+
+ cr.stroke()
+
+
+def draw_rockets(cr):
+ # BASE Constants
+ X_BASE = mToPx(2.41568)
+ Y_BASE = 0
+ # Robot longitudinal radius
+ R = 0.381
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+ near_side_rocket_center[1] - 0.8 * mToPx(0.5))
+ cr.move_to(near_side_rocket_center[0] - R * mToPx(0.866),
+ near_side_rocket_center[1] - R * mToPx(0.5))
+ cr.arc(near_side_rocket_center[0] - R * mToPx(0.866),
+ near_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.8))
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R))
+ cr.arc(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+ far_side_rocket_center[1] - 0.8 * mToPx(0.5))
+ cr.move_to(far_side_rocket_center[0] + R * mToPx(0.866),
+ far_side_rocket_center[1] - R * mToPx(0.5))
+ cr.arc(far_side_rocket_center[0] + R * mToPx(0.866),
+ far_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+ #print(far_side_rocket_center)
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+ near_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.8))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+ far_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Bottom Rocket
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+
+def draw_cargo_ship(cr):
+ # BASE Constants
+ X_BASE = 0 + mToPx(5.59435)
+ Y_BASE = 0 + 0 #mToPx(4.129151)
+ R = 0.381 - 0.1
+
+ FRONT_PEG_DELTA_Y = mToPx(0.276352)
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.8), Y_BASE + FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.arc(X_BASE - mToPx(R), Y_BASE + FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.8), Y_BASE - FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.arc(X_BASE - mToPx(R), Y_BASE - FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+ SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+ SIDE_PEG_X = X_BASE + mToPx(1.148842)
+ SIDE_PEG_DX = mToPx(0.55245)
+
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+ mToPx(1.41605))
+ cr.stroke()
+
+
+def draw_points(cr, p, size):
+ for i in range(0, len(p)):
+ draw_px_cross(cr, p[i][0], p[i][1], size,
+ Color(0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
new file mode 100644
index 0000000..7e98a38
--- /dev/null
+++ b/frc971/control_loops/python/graph.py
@@ -0,0 +1,178 @@
+from constants import *
+import cairo
+from color import Color, palette
+from points import Points
+from drawing_constants import *
+from libspline import Spline, DistanceSpline, Trajectory
+
+AXIS_MARGIN_SPACE = 40
+
+
+class Graph(): # (TODO): Remove Computer Calculation
+ def __init__(self, cr, mypoints):
+ # Background Box
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+ SCREEN_SIZE * 0.6)
+ cr.fill()
+
+ cr.set_source_rgb(0, 0, 0)
+ cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+ SCREEN_SIZE * 0.6)
+ #Axis
+ cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE) # Y
+ cr.line_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ 0.1 * SCREEN_SIZE - 10)
+
+ cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE) # X
+ cr.line_to(-10, -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE)
+ cr.stroke()
+
+ skip = 2
+ dT = 0.00505
+ start = AXIS_MARGIN_SPACE - SCREEN_SIZE
+ end = -2.0 * AXIS_MARGIN_SPACE
+ height = 0.5 * (SCREEN_SIZE) - AXIS_MARGIN_SPACE
+ zero = AXIS_MARGIN_SPACE - SCREEN_SIZE / 2.0
+ if mypoints.getLibsplines():
+ distanceSpline = DistanceSpline(mypoints.getLibsplines())
+ traj = Trajectory(distanceSpline)
+ traj.Plan()
+ XVA = traj.GetPlanXVA(dT)
+ self.draw_x_axis(cr, start, height, zero, XVA, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawAcceleration(cr, XVA, start, height, skip, zero,
+ AXIS_MARGIN_SPACE, end)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+ cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+ cr.stroke()
+
+ def connectLines(self, cr, points, color):
+ for i in range(0, len(points) - 1):
+ set_color(cr, color)
+ cr.move_to(points[i][0], points[i][1])
+ cr.line_to(points[i + 1][0], points[i + 1][1])
+ cr.stroke()
+
+ def draw_x_axis(self, cr, start, height, zero, xva, end):
+ total_time = 0.00505 * len(xva[0])
+ for k in np.linspace(0, 1, 11):
+ self.tickMark(cr,
+ k * np.abs(start - end) + start, zero + height / 2.0,
+ 10, palette["BLACK"])
+ cr.move_to(k * np.abs(start - end) + start,
+ 10 + zero + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(k * total_time, 3)), txt_scale,
+ txt_scale, 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def tickMark(self, cr, x, y, height, COLOR):
+ # X, Y is in the middle of the tick mark
+ set_color(cr, COLOR)
+ cr.move_to(x, y + (height / 2))
+ cr.line_to(x, y - (height / 2))
+ cr.stroke()
+
+ def HtickMark(self, cr, x, y, width, COLOR):
+ # X, Y is in the middle of the tick mark
+ set_color(cr, COLOR)
+ cr.move_to(x + (width / 2), y)
+ cr.line_to(x - (width / 2), y)
+ cr.stroke()
+
+ def drawVelocity(self, cr, xva, start, height, skip, zero, end):
+ COLOR = palette["RED"]
+ velocity = xva[1]
+ n_timesteps = len(velocity)
+ max_v = np.amax(velocity)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ scaler = height / max_v
+ cr.set_source_rgb(1, 0, 0)
+ points = []
+ for i in range(0, len(velocity)):
+ if i % skip == 0:
+ points.append([
+ start + (i * spacing),
+ zero + height / 2.0 + (velocity[i] * scaler / 2.0)
+ ])
+ self.connectLines(cr, points, COLOR)
+
+ # draw axes marking
+ for i in np.linspace(-1, 1, 11):
+ self.HtickMark(cr, start, zero + i * height / 2.0 + height / 2.0,
+ 10, palette["BLACK"])
+ cr.set_source_rgb(1, 0, 0)
+ cr.move_to(start + 5, zero + i * height / 2.0 + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * max_v, 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def drawAcceleration(self, cr, xva, start, height, skip, zero, margin,
+ end):
+ COLOR = palette["BLUE"]
+ accel = xva[2]
+ max_a = np.amax(accel)
+ min_a = np.amin(accel)
+ n_timesteps = len(accel)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ scaler = height / (max_a - min_a)
+ cr.set_source_rgb(1, 0, 0)
+ points = []
+ for i in range(0, len(accel)):
+ if i % skip == 0:
+ points.append([
+ start + (i * spacing), zero + ((accel[i] - min_a) * scaler)
+ ])
+ self.connectLines(cr, points, COLOR)
+
+ # draw axes marking
+ for i in np.linspace(0, 1, 11):
+ self.HtickMark(cr, -1.5 * margin, zero + i * height, 10,
+ palette["BLACK"])
+ cr.set_source_rgb(0, 0, 1)
+ cr.move_to(-1.2 * margin, zero + i * height)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * (max_a - min_a) + min_a,
+ 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def drawVoltage(self, cr, xva, start, height, skip, traj, zero, end):
+ COLOR1 = palette["GREEN"]
+ COLOR2 = palette["CYAN"]
+ poses = xva[0]
+ n_timesteps = len(poses)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ points1 = []
+ points2 = []
+ for i in range(0, len(poses)):
+ if i % skip == 0:
+ voltage = traj.Voltage(poses[i])
+ points1.append([
+ start + (i * spacing),
+ zero + height / 2 + height * (voltage[0] / 24.0)
+ ])
+ points2.append([
+ start + (i * spacing),
+ zero + height / 2 + height * (voltage[1] / 24.0)
+ ])
+ self.connectLines(cr, points1, COLOR1)
+ self.connectLines(cr, points2, COLOR2)
+
+ for i in np.linspace(-1, 1, 7):
+ self.HtickMark(cr, -1.0 * SCREEN_SIZE,
+ zero + i * height / 2.0 + height / 2.0, 10,
+ palette["BLACK"])
+ cr.set_source_rgb(0, 1, 1)
+ cr.move_to(-1.0 * SCREEN_SIZE,
+ zero + i * height / 2.0 + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * 12.0, 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
old mode 100644
new mode 100755
index e59286f..0d1b263
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -3,67 +3,23 @@
import os
import sys
import copy
-import basic_window
from color import Color, palette
import random
import gi
import numpy as np
-from libspline import Spline
import scipy.spatial.distance
gi.require_version('Gtk', '3.0')
+gi.require_version('Gdk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
+from libspline import Spline, DistanceSpline, Trajectory
import enum
-import json # For writing to json files
-
-from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-
-WIDTH_OF_FIELD_IN_METERS = 8.258302
-PIXELS_ON_SCREEN = 300
-
-
-def pxToM(p):
- return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-
-
-def mToPx(m):
- return (m*PIXELS_ON_SCREEN/WIDTH_OF_FIELD_IN_METERS)
-
-def px(cr):
- return OverrideMatrix(cr, identity)
-
-
-def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
- """Draws a cross with fixed dimensions in pixel space."""
- set_color(cr, color)
- cr.move_to(x, y - length_px)
- cr.line_to(x, y + length_px)
- cr.stroke()
-
- cr.move_to(x - length_px, y)
- cr.line_to(x + length_px, y)
- cr.stroke()
- set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
- """Draws a x with fixed dimensions in pixel space."""
- length_px = length_px1 / np.sqrt(2)
- set_color(cr, color)
- cr.move_to(x - length_px, y - length_px)
- cr.line_to(x + length_px, y + length_px)
- cr.stroke()
-
- cr.move_to(x - length_px, y + length_px)
- cr.line_to(x + length_px, y - length_px)
- cr.stroke()
- set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_points(cr, p, size):
- for i in range(0, len(p)):
- draw_px_cross(cr, p[i][0], p[i][1], size, Color(
- 0, np.sqrt(0.2 * i), 0))
+import json
+from basic_window import *
+from constants import *
+from drawing_constants import *
+from points import Points
+from graph import Graph
class Mode(enum.Enum):
@@ -72,61 +28,25 @@
kEditing = 2
kExporting = 3
kImporting = 4
- kConstraint = 5
-class ConstraintType(enum.Enum):
- kMaxSpeed = 0
- kMaxAcceleration = 1
-
-
-def display_text(cr, text, widtha, heighta, widthb, heightb):
- cr.scale(widtha, -heighta)
- cr.show_text(text)
- cr.scale(widthb, -heightb)
-
-
-def redraw(needs_redraw, window):
- print("Redrew")
- if not needs_redraw:
- window.queue_draw()
-
-
-class Constraint():
- def __init__(self, start, end, constraint, value):
- self.start = start #Array with index and distance from start of spline
- self.end = end #Array with index and distance from start of spline
- self.constraint = constraint #INT
- self.value = value #INT
- if self.constraint == 0:
- self.conName = "kMaxSpeed"
- else:
- self.conName = "kMaxAcceleration"
-
- def toString(self):
-
- return "START: " + str(self.start[0]) + ", " + str(
- self.start[1]) + " | END: " + str(self.end[0]) + ", " + str(
- self.end[1]) + " | " + str(self.conName) + ": " + str(
- self.value)
-
-
-class GTK_Widget(basic_window.BaseWindow):
+class GTK_Widget(BaseWindow):
"""Create a GTK+ widget on which we will draw using Cairo"""
-
def __init__(self):
super(GTK_Widget, self).__init__()
+ self.points = Points()
+
# init field drawing
# add default spline for testing purposes
# init editing / viewing modes and pointer location
self.mode = Mode.kPlacing
self.x = 0
self.y = 0
-
module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
self.path_to_export = os.path.join(module_path,
- 'points_for_pathedit.json')
+ 'points_for_pathedit.json')
+
# update list of control points
self.point_selected = False
# self.adding_spline = False
@@ -145,32 +65,20 @@
for c in palette:
self.colors.append(palette[c])
- self.selected_points = []
- self.splines = []
self.reinit_extents()
self.inStart = None
self.inEnd = None
- self.inConstraint = None
self.inValue = None
self.startSet = False
- #John also wrote this
- def add_point(self, x, y):
- if (len(self.selected_points) < 6):
- self.selected_points.append([pxToM(x), pxToM(y)])
- if (len(self.selected_points) == 6):
- self.mode = Mode.kEditing
- self.splines.append(np.array(self.selected_points))
- self.selected_points = []
-
"""set extents on images"""
def reinit_extents(self):
- self.extents_x_min = -800
- self.extents_x_max = 800
- self.extents_y_min = -800
- self.extents_y_max = 800
+ self.extents_x_min = -1.0 * SCREEN_SIZE
+ self.extents_x_max = SCREEN_SIZE
+ self.extents_y_min = -1.0 * SCREEN_SIZE
+ self.extents_y_max = SCREEN_SIZE
# this needs to be rewritten with numpy, i dont think this ought to have
# SciPy as a dependecy
@@ -184,192 +92,112 @@
def get_nearest_point(self):
return self.all_controls[self.get_index_of_nearest_point()]
- def set_index_to_nearest_spline_point(self):
- nearest = 50
- index_of_closest = 0
- self.spline_edit = 0
- cur_p = [self.x, self.y]
-
- for index_splines, points in enumerate(self.spline):
- for index_points, point in enumerate(points.curve):
- # pythagorean
- distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
- (cur_p[1] - mToPx(point[1])**2))
- if distance < nearest:
- nearest = distance
- print("DISTANCE: ", distance, " | INDEX: ", index_points)
- index_of_closest = index_points
- self.index_of_edit = index_of_closest
- self.spline_edit = index_splines
- self.held_x = self.x
- if self.startSet == False:
- self.inStart = [self.index_of_edit, self.findDistance()]
- self.startSet = True
- else:
- self.inEnd = [self.index_of_edit, self.findDistance()]
- self.startSet = False
- self.mode = Mode.kEditing
- self.spline_edit = -1
- self.index_of_edit = -1
-
- print("Nearest: " + str(nearest))
- print("Spline: " + str(self.spline_edit))
- print("Index: " + str(index_of_closest))
-
- def findDistance(self):
- """ findDistance goes through each point on the spline finding distance to that point from the point before.
- It does this to find the the length of the spline to the point that is currently selected.
- """
- distance = 0
- points = self.curves[self.spline_edit]
- for index, point in enumerate(points):
- if index > 0 and index <= self.index_of_edit:
- distance += np.sqrt((points[index - 1][0] - point[0])**2 +
- (points[index - 1][1] - point[1])**2)
- return distance
-
- def draw_HAB(self, cr):
- print("WENT IN")
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- BACKWALL_X = -450
-
- # HAB Levels 2 and 3 called in variables backhab
-
- WIDTH_BACKHAB = mToPx(1.2192)
-
- Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
- BACKHAB_LV2_LENGTH = mToPx(1.016)
-
- BACKHAB_LV3_LENGTH = mToPx(1.2192)
- Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
-
- Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX -BACKHAB_LV2_LENGTH
-
- # HAB LEVEL 1
- X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
-
- WIDTH_LV1_BOX = mToPx(0.90805)
- LENGTH_LV1_BOX = mToPx(1.6256)
-
- Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
-
- # Ramp off Level 1
- X_RAMP = X_LV1_BOX
-
- Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
- WIDTH_TOP_RAMP = mToPx(1.20015)
- LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
-
- X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
- Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
- LENGTH_MIDDLE_RAMP = 2*LENGTH_LV1_BOX
- WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
-
- Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
-
- # Side Bars to Hold in balls
- X_BARS = BACKWALL_X
- WIDTH_BARS = WIDTH_BACKHAB
- LENGTH_BARS = mToPx(0.574675)
-
- Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
-
- Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
-
- set_color(cr, palette["BLACK"])
- cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB,
- BACKHAB_LV3_LENGTH)
- cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
- cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX,
- LENGTH_LV1_BOX)
- cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
- LENGTH_MIDDLE_RAMP)
- cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.stroke()
- #draw_px_x(cr, BACKWALL_X, 0, 10) # Midline Point
- #draw_px_x(cr, X_BASE, Y_BASE, 10) # Bases
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
-
- cr.stroke()
-
- def draw_rockets(self, cr):
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- # Top Rocket
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- # Bottom Rocket
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- cr.stroke()
-
- def draw_cargo_ship(self, cr):
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
- mToPx(2.43205), mToPx(1.41605))
-
- cr.stroke()
-
def draw_field_elements(self, cr):
- self.draw_HAB(cr)
- self.draw_rockets(cr)
- self.draw_cargo_ship(cr)
+ draw_HAB(cr)
+ draw_rockets(cr)
+ draw_cargo_ship(cr)
- def handle_draw(self, cr):
- # print(self.new_point)
- # print("SELF.POINT_SELECTED: " + str(self.point_selected))
+ def draw_robot_at_point(self, cr, i, p, spline):
+ p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
+ p2 = [mToPx(spline.Point(i + p)[0]), mToPx(spline.Point(i + p)[1])]
- # begin drawing
+ #Calculate Robot
+ 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
+
+ front_middle = []
+ front_middle.append(p1[0] + x_difference)
+ front_middle.append(p1[1] + y_difference)
+
+ back_middle = []
+ back_middle.append(p1[0] - x_difference)
+ back_middle.append(p1[1] - y_difference)
+
+ 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)
+
+ front_1 = []
+ front_1.append(front_middle[0] - x_difference)
+ front_1.append(front_middle[1] - y_difference)
+
+ front_2 = []
+ front_2.append(front_middle[0] + x_difference)
+ front_2.append(front_middle[1] + y_difference)
+
+ back_1 = []
+ back_1.append(back_middle[0] - x_difference)
+ back_1.append(back_middle[1] - y_difference)
+
+ back_2 = []
+ back_2.append(back_middle[0] + x_difference)
+ back_2.append(back_middle[1] + y_difference)
+
+ x_difference = x_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+ y_difference = y_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+
+ #Calculate Ball
+ ball_center = []
+ ball_center.append(p1[0] + x_difference)
+ ball_center.append(p1[1] + y_difference)
+
+ x_difference = x_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+ y_difference = y_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+
+ #Calculate Panel
+ panel_center = []
+ panel_center.append(p1[0] + x_difference)
+ panel_center.append(p1[1] + y_difference)
+
+ x_difference = np.sin(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+ y_difference = np.cos(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+
+ panel_1 = []
+ panel_1.append(panel_center[0] + x_difference)
+ panel_1.append(panel_center[1] + y_difference)
+
+ panel_2 = []
+ panel_2.append(panel_center[0] - x_difference)
+ panel_2.append(panel_center[1] - y_difference)
+
+ #Draw Robot
+ cr.move_to(front_1[0], front_1[1])
+ cr.line_to(back_1[0], back_1[1])
+ cr.line_to(back_2[0], back_2[1])
+ cr.line_to(front_2[0], front_2[1])
+ cr.line_to(front_1[0], front_1[1])
+
+ cr.stroke()
+
+ #Draw Ball
+ set_color(cr, palette["ORANGE"], 0.5)
+ cr.move_to(back_middle[0], back_middle[1])
+ cr.line_to(ball_center[0], ball_center[1])
+ cr.arc(ball_center[0], ball_center[1], mToPx(BALL_RADIUS), 0,
+ 2 * np.pi)
+ cr.stroke()
+
+ #Draw Panel
+ set_color(cr, palette["YELLOW"], 0.5)
+ cr.move_to(panel_1[0], panel_1[1])
+ cr.line_to(panel_2[0], panel_2[1])
+
+ cr.stroke()
+ cr.set_source_rgba(0, 0, 0, 1)
+
+ def handle_draw(self, cr): # main
# Fill the background color of the window with grey
- set_color(cr, palette["GREY"])
+ set_color(cr, palette["WHITE"])
cr.paint()
- #Scale the field to fit within drawing area
- cr.scale(0.5, 0.5)
# Draw a extents rectangle
set_color(cr, palette["WHITE"])
@@ -378,209 +206,181 @@
self.extents_y_max - self.extents_y_min)
cr.fill()
- #Drawing the field
+ #Drawing the switch and scale in the field
cr.move_to(0, 50)
cr.show_text('Press "e" to export')
cr.show_text('Press "i" to import')
- set_color(cr, Color(0.3, 0.3, 0.3))
- cr.rectangle(-450, -150, 300, 300)
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
cr.fill()
set_color(cr, palette["BLACK"])
- cr.rectangle(-450, -150, 300, 300)
+ cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
-
self.draw_field_elements(cr)
-
y = 0
- # update all the things
+ # update everything
- if self.mode == Mode.kViewing:
+ if self.mode == Mode.kPlacing or self.mode == Mode.kViewing:
set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- cr.show_text("VIEWING")
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- # for item in self.selected_points:
- # print(str(item))
- for i, point in enumerate(self.selected_points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
- elif self.mode == Mode.kPlacing:
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "ADD", 1, 1, 1, 1)
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- for item in self.selected_points:
- print(str(item))
- for i, point in enumerate(self.selected_points):
- print("I: " + str(i))
+ cr.move_to(-SCREEN_SIZE, 170)
+ plotPoints = self.points.getPoints()
+ if plotPoints:
+ for i, point in enumerate(plotPoints):
draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ set_color(cr, palette["WHITE"])
elif self.mode == Mode.kEditing:
set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
+ cr.move_to(-SCREEN_SIZE, 170)
display_text(cr, "EDITING", 1, 1, 1, 1)
- if len(self.splines) > 0:
- holder_spline = []
- for i, points in enumerate(self.splines):
- array = np.zeros(shape=(6, 2), dtype=float)
- for j, point in enumerate(points):
- array[j, 0] = mToPx(point[0])
- array[j, 1] = mToPx(point[1])
- spline = Spline(np.ascontiguousarray(np.transpose(array)))
- for k in np.linspace(0.01, 1, 100):
- cr.move_to(
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1])
- cr.line_to(spline.Point(k)[0], spline.Point(k)[1])
- cr.stroke()
- holding = [
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1]
- ]
- holder_spline.append(holding)
- self.curves.append(holder_spline)
+ if self.points.getSplines():
+ self.draw_splines(cr)
+ for i, points in enumerate(self.points.getSplines()):
- for spline, points in enumerate(self.splines):
- # for item in points:
- # print(str(item))
- for i, point in enumerate(points):
- # print("I: " + str(i))
- if spline == self.spline_edit and i == self.index_of_edit:
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
- self.colors[spline])
- elif (spline == 0 and not i == 5) or (not i == 0
- and not i == 5):
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
- self.colors[spline])
- cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ p0 = np.array([mToPx(points[0][0]), mToPx(points[0][1])])
+ p1 = np.array([mToPx(points[1][0]), mToPx(points[1][1])])
+ p2 = np.array([mToPx(points[2][0]), mToPx(points[2][1])])
+ p3 = np.array([mToPx(points[3][0]), mToPx(points[3][1])])
+ p4 = np.array([mToPx(points[4][0]), mToPx(points[4][1])])
+ p5 = np.array([mToPx(points[5][0]), mToPx(points[5][1])])
- elif self.mode == Mode.kConstraint:
- print("Drawn")
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "Adding Constraint", 1, 1, 1, 1)
- if len(self.splines) > 0:
- # print("Splines: " + str(len(self.splines)))
- # print("ITEMS:")
- for s, points in enumerate(self.splines):
- # for item in points:
- # print(str(item))
- for i, point in enumerate(points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10, self.colors[s])
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ draw_control_points(cr, [p0, p1, p2, p3, p4, p5])
+ first_tangent = p0 + 2.0 * (p1 - p0)
+ second_tangent = p5 + 2.0 * (p4 - p5)
+ cr.set_source_rgb(0, 0.5, 0)
+ cr.move_to(p0[0], p0[1])
+ cr.set_line_width(1.0)
+ cr.line_to(first_tangent[0], first_tangent[1])
+ cr.move_to(first_tangent[0], first_tangent[1])
+ cr.line_to(p2[0], p2[1])
- cr.paint_with_alpha(.65)
+ cr.move_to(p5[0], p5[1])
+ cr.line_to(second_tangent[0], second_tangent[1])
+ cr.move_to(second_tangent[0], second_tangent[1])
+ cr.line_to(p3[0], p3[1])
+
+ cr.stroke()
+ cr.set_line_width(2.0)
+ self.points.update_lib_spline()
+ set_color(cr, palette["WHITE"])
+
+ cr.paint_with_alpha(0.2)
+
+ mygraph = Graph(cr, self.points)
draw_px_cross(cr, self.x, self.y, 10)
- def do_key_press(self, event):
+ def draw_splines(self, cr):
+ holder_spline = []
+ for i, points in enumerate(self.points.getSplines()):
+ array = np.zeros(shape=(6, 2), dtype=float)
+ for j, point in enumerate(points):
+ array[j, 0] = point[0]
+ array[j, 1] = point[1]
+ spline = Spline(np.ascontiguousarray(np.transpose(array)))
+ for k in np.linspace(0.01, 1, 100):
+ cr.move_to(mToPx(spline.Point(k - 0.01)[0]),
+ mToPx(spline.Point(k - 0.01)[1]))
+ cr.line_to(mToPx(spline.Point(k)[0]),
+ mToPx(spline.Point(k)[1]))
+ cr.stroke()
+ holding = [
+ spline.Point(k - 0.01)[0],
+ spline.Point(k - 0.01)[1]
+ ]
+ holder_spline.append(holding)
+ if i == 0:
+ self.draw_robot_at_point(cr, 0.00, 0.01, spline)
+ self.draw_robot_at_point(cr, 1, 0.01, spline)
+ self.curves.append(holder_spline)
+
+ def mouse_move(self, event):
+ old_x = self.x
+ old_y = self.y
+ self.x = event.x
+ self.y = event.y
+ dif_x = event.x - old_x
+ dif_y = event.y - old_y
+ difs = np.array([pxToM(dif_x), pxToM(dif_y)])
+
+ if self.mode == Mode.kEditing:
+ self.spline_edit = self.points.updates_for_mouse_move(
+ self.index_of_edit, self.spline_edit, self.x, self.y, difs)
+
+ def do_key_press(self, event, file_name):
keyval = Gdk.keyval_to_lower(event.keyval)
- # print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+ self.path_to_export = os.path.join(module_path,
+ "spline_jsons/" + file_name)
if keyval == Gdk.KEY_q:
print("Found q key and exiting.")
quit_main_loop()
- if keyval == Gdk.KEY_e:
- # Will export to json file
- self.mode = Mode.kEditing
- print(str(sys.argv))
- print('out to: ', self.path_to_export)
- exportList = [l.tolist() for l in self.splines]
- with open(self.path_to_export, mode='w') as points_file:
- json.dump(exportList, points_file)
- print("Wrote: " + str(self.splines))
- if keyval == Gdk.KEY_i:
- # import from json file
- self.mode = Mode.kEditing
- self.selected_points = []
- self.splines = []
- with open(self.path_to_export) as points_file:
- self.splines = json.load(points_file)
- print("Added: " + str(self.splines))
+ file_name_end = file_name[-5:]
+ if file_name_end != ".json":
+ print("Error: Filename doesn't end in .json")
+ else:
+ if keyval == Gdk.KEY_e:
+ # Will export to json file
+ self.mode = Mode.kEditing
+ print('out to: ', self.path_to_export)
+ exportList = [l.tolist() for l in self.points.getSplines()]
+ with open(self.path_to_export, mode='w') as points_file:
+ json.dump(exportList, points_file)
+
+ if keyval == Gdk.KEY_i:
+ # import from json file
+ self.mode = Mode.kEditing
+ self.points.resetPoints()
+ self.points.resetSplines()
+ with open(self.path_to_export) as points_file:
+ self.points.setUpSplines(json.load(points_file))
+
+ self.points.update_lib_spline()
+
if keyval == Gdk.KEY_p:
self.mode = Mode.kPlacing
# F0 = A1
# B1 = 2F0 - E0
# C1= d0 + 4F0 - 4E0
- spline_index = len(self.splines) - 1
- self.selected_points = []
- f = self.splines[spline_index][5]
- e = self.splines[spline_index][4]
- d = self.splines[spline_index][3]
- self.selected_points.append(f)
- self.selected_points.append(f * 2 + e * -1)
- self.selected_points.append(d + f * 4 + e * -4)
-
- if keyval == Gdk.KEY_c:
- self.mode = Mode.kConstraint
+ spline_index = len(self.points.getSplines()) - 1
+ self.points.resetPoints()
+ self.points.extrapolate(
+ self.points.getSplines()[len(self.points.getSplines()) - 1][5],
+ self.points.getSplines()[len(self.points.getSplines()) - 1][4],
+ self.points.getSplines()[len(self.points.getSplines()) - 1][3])
def button_press_action(self):
if self.mode == Mode.kPlacing:
- #Check that the point clicked is on the field
- if (self.x < -150 and self.x > -450 and self.y < 150
- and self.y > -150):
- self.add_point(self.x, self.y)
+ if self.points.add_point(self.x, self.y):
+ self.mode = Mode.kEditing
elif self.mode == Mode.kEditing:
# Now after index_of_edit is not -1, the point is selected, so
# user can click for new point
if self.index_of_edit > -1 and self.held_x != self.x:
- print("INDEX OF EDIT: " + str(self.index_of_edit))
- self.splines[self.spline_edit][self.index_of_edit] = [
- pxToM(self.x), pxToM(self.y)
- ]
+ self.points.setSplines(self.spline_edit, self.index_of_edit,
+ pxToM(self.x), pxToM(self.y))
- if not self.spline_edit == len(self.splines) - 1:
- spline_edit = self.spline_edit + 1
- f = self.splines[self.spline_edit][5]
- e = self.splines[self.spline_edit][4]
- d = self.splines[self.spline_edit][3]
- self.splines[spline_edit][0] = f
- self.splines[spline_edit][1] = f * 2 + e * -1
- self.splines[spline_edit][2] = d + f * 4 + e * -4
-
- if not self.spline_edit == 0:
- spline_edit = self.spline_edit - 1
- a = self.splines[self.spline_edit][0]
- b = self.splines[self.spline_edit][1]
- c = self.splines[self.spline_edit][2]
- self.splines[spline_edit][5] = a
- self.splines[spline_edit][4] = a * 2 + b * -1
- self.splines[spline_edit][3] = c + a * 4 + b * -4
+ self.spline_edit = self.points.splineExtrapolate(
+ self.spline_edit)
self.index_of_edit = -1
self.spline_edit = -1
else:
- print("mode == 2")
# Get clicked point
# Find nearest
# Move nearest to clicked
cur_p = [pxToM(self.x), pxToM(self.y)]
- print("CUR_P: " + str(self.x) + " " + str(self.y))
# Get the distance between each for x and y
# Save the index of the point closest
nearest = 1 # Max distance away a the selected point can be in meters
index_of_closest = 0
- for index_splines, points in enumerate(self.splines):
+ for index_splines, points in enumerate(self.points.getSplines()):
for index_points, val in enumerate(points):
- # pythagorean
distance = np.sqrt((cur_p[0] - val[0])**2 +
(cur_p[1] - val[1])**2)
if distance < nearest:
@@ -591,10 +391,6 @@
self.index_of_edit = index_of_closest
self.spline_edit = index_splines
self.held_x = self.x
- elif self.mode == Mode.kConstraint:
- print("RAN")
- self.set_index_to_nearest_spline_point()
- print("FINISHED")
def do_button_press(self, event):
print("button press activated")
@@ -612,13 +408,23 @@
print("Method_connect ran")
self.connect(event, handler)
+ def mouse_move(self, event):
+ #Changes event.x and event.y to be relative to the center.
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.mouse_move(event)
+ self.queue_draw()
+
def button_press(self, event):
print("button press activated")
o_x = event.x
o_y = event.y
x = event.x - self.drawing_area.window_shape[0] / 2
y = self.drawing_area.window_shape[1] / 2 - event.y
- scale = self.drawing_area.get_current_scale()
+ scale = 2 * self.drawing_area.get_current_scale()
event.x = x / scale + self.drawing_area.center[0]
event.y = y / scale + self.drawing_area.center[1]
self.drawing_area.do_button_press(event)
@@ -627,21 +433,17 @@
def key_press(self, event):
print("key press activated")
- self.drawing_area.do_key_press(event)
+ self.drawing_area.do_key_press(event, self.file_name_box.get_text())
self.queue_draw()
def configure(self, event):
print("configure activated")
self.drawing_area.window_shape = (event.width, event.height)
- def on_submit_click(self, widget):
- self.drawing_area.inConstraint = int(self.constraint_box.get_text())
- self.drawing_area.inValue = int(self.value_box.get_text())
-
def __init__(self):
Gtk.Window.__init__(self)
- self.set_default_size(1366, 738)
+ self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
flowBox = Gtk.FlowBox()
flowBox.set_valign(Gtk.Align.START)
@@ -663,41 +465,25 @@
| Gdk.EventMask.SCROLL_MASK
| Gdk.EventMask.KEY_PRESS_MASK)
+ #add the graph box
self.drawing_area = GTK_Widget()
self.eventBox.add(self.drawing_area)
self.method_connect("key-release-event", self.key_press)
self.method_connect("button-release-event", self.button_press)
self.method_connect("configure-event", self.configure)
+ self.method_connect("motion_notify_event", self.mouse_move)
- # Constraint Boxes
+ self.file_name_box = Gtk.Entry()
+ self.file_name_box.set_size_request(200, 40)
- self.start_box = Gtk.Entry()
- self.start_box.set_size_request(100, 20)
+ self.file_name_box.set_text("File")
+ self.file_name_box.set_editable(True)
- self.constraint_box = Gtk.Entry()
- self.constraint_box.set_size_request(100, 20)
-
- self.constraint_box.set_text("Constraint")
- self.constraint_box.set_editable(True)
-
- container.put(self.constraint_box, 700, 0)
-
- self.value_box = Gtk.Entry()
- self.value_box.set_size_request(100, 20)
-
- self.value_box.set_text("Value")
- self.value_box.set_editable(True)
-
- container.put(self.value_box, 700, 40)
-
- self.submit_button = Gtk.Button("Submit")
- self.submit_button.connect('clicked', self.on_submit_click)
-
- container.put(self.submit_button, 880, 0)
+ container.put(self.file_name_box, 0, 0)
self.show_all()
window = GridWindow()
-basic_window.RunApp()
+RunApp()
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
new file mode 100644
index 0000000..8f94e3f
--- /dev/null
+++ b/frc971/control_loops/python/points.py
@@ -0,0 +1,110 @@
+from constants import *
+import numpy as np
+from libspline import Spline, DistanceSpline, Trajectory
+
+
+class Points():
+ def __init__(self):
+ self.points = [] # Holds all points not yet in spline
+ self.libsplines = [] # Formatted for libspline library usage
+ self.splines = [] # Formatted for drawing
+
+ def getPoints(self):
+ return self.points
+
+ def resetPoints(self):
+ self.points = []
+
+ def getLibsplines(self):
+ return self.libsplines
+
+ def splineExtrapolate(self, o_spline_edit):
+ spline_edit = o_spline_edit
+ if not spline_edit == len(self.splines) - 1:
+ spline_edit = spline_edit + 1
+ f = self.splines[spline_edit][5]
+ e = self.splines[spline_edit][4]
+ d = self.splines[spline_edit][3]
+ self.splines[spline_edit][0] = f
+ self.splines[spline_edit][1] = f * 2 + e * -1
+ self.splines[spline_edit][2] = d + f * 4 + e * -4
+
+ if not spline_edit == 0:
+ spline_edit = spline_edit - 1
+ a = self.splines[spline_edit][0]
+ b = self.splines[spline_edit][1]
+ c = self.splines[spline_edit][2]
+ self.splines[spline_edit][5] = a
+ self.splines[spline_edit][4] = a * 2 + b * -1
+ self.splines[spline_edit][3] = c + a * 4 + b * -4
+
+ return spline_edit
+
+ def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs):
+ if index_of_edit > -1:
+ self.splines[spline_edit][index_of_edit] = [pxToM(x), pxToM(y)]
+
+ if index_of_edit == 5:
+ self.splines[spline_edit][
+ index_of_edit -
+ 2] = self.splines[spline_edit][index_of_edit - 2] + difs
+ self.splines[spline_edit][
+ index_of_edit -
+ 1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+ if index_of_edit == 0:
+ self.splines[spline_edit][
+ index_of_edit +
+ 2] = self.splines[spline_edit][index_of_edit + 2] + difs
+ self.splines[spline_edit][
+ index_of_edit +
+ 1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+ if index_of_edit == 4:
+ self.splines[spline_edit][
+ index_of_edit -
+ 1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+ if index_of_edit == 1:
+ self.splines[spline_edit][
+ index_of_edit +
+ 1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+ return self.splineExtrapolate(spline_edit)
+
+ def update_lib_spline(self):
+ self.libsplines = []
+ array = np.zeros(shape=(6, 2), dtype=float)
+ for points in self.splines:
+ for j, point in enumerate(points):
+ array[j, 0] = point[0]
+ array[j, 1] = point[1]
+ spline = Spline(np.ascontiguousarray(np.transpose(array)))
+ self.libsplines.append(spline)
+
+ def getSplines(self):
+ return self.splines
+
+ def resetSplines(self):
+ self.splines = []
+
+ def setUpSplines(self, newSplines):
+ self.splines = newSplines
+
+ def setSplines(self, spline_edit, index_of_edit, x, y):
+ self.splines[spline_edit][index_of_edit] = [x, y]
+
+ def add_point(self, x, y):
+ if (len(self.points) < 6):
+ self.points.append([pxToM(x), pxToM(y)])
+ if (len(self.points) == 6):
+ self.splines.append(np.array(self.points))
+ self.points = []
+ self.update_lib_spline()
+ return True
+
+ def extrapolate(self, point1, point2,
+ point3): # where point3 is 3rd to last point
+ self.points.append(point1)
+ self.points.append(point1 * 2 - point2)
+ self.points.append(point3 + point1 * 4 - point2 * 4)
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index e69d874..b428c9a 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -15,7 +15,6 @@
from frc971.control_loops.python import drivetrain
from frc971.control_loops.python import controls
import y2016.control_loops.python.drivetrain
-
"""This file is my playground for implementing spline following.
All splines here are cubic bezier splines. See
@@ -73,8 +72,9 @@
if numpy.isscalar(alpha):
alpha = [alpha]
dalpha_matrix = [[
- -3.0 * (1.0 - a)**2.0, 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 *
- (1.0 - a) * a, -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
+ -3.0 * (1.0 - a)**2.0,
+ 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 * (1.0 - a) * a,
+ -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
] for a in alpha]
return control_points * numpy.matrix(dalpha_matrix).T
@@ -97,8 +97,7 @@
ddalpha_matrix = [[
2.0 * 3.0 * (1.0 - a),
-2.0 * 3.0 * (1.0 - a) + -2.0 * 3.0 * (1.0 - a) + 2.0 * 3.0 * a,
- -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a,
- 2.0 * 3.0 * a
+ -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a, 2.0 * 3.0 * a
] for a in alpha]
return control_points * numpy.matrix(ddalpha_matrix).T
@@ -119,10 +118,8 @@
if numpy.isscalar(alpha):
alpha = [alpha]
ddalpha_matrix = [[
- -2.0 * 3.0,
- 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
- -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0,
- 2.0 * 3.0
+ -2.0 * 3.0, 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
+ -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0
] for a in alpha]
return control_points * numpy.matrix(ddalpha_matrix).T
@@ -143,7 +140,8 @@
dspline_points = dspline(alpha, control_points)
return numpy.arctan2(
- numpy.array(dspline_points)[1, :], numpy.array(dspline_points)[0, :])
+ numpy.array(dspline_points)[1, :],
+ numpy.array(dspline_points)[0, :])
def dspline_theta(alpha,
@@ -219,8 +217,8 @@
dddy = numpy.array(dddspline_points)[1, :]
return -1.0 / ((dx**2.0 + dy**2.0)**2.0) * (dx * ddy - dy * ddx) * 2.0 * (
- dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy - dy *
- dddx)
+ dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy -
+ dy * dddx)
class Path(object):
@@ -230,7 +228,8 @@
self._control_points = control_points
def spline_velocity(alpha):
- return numpy.linalg.norm(dspline(alpha, self._control_points), axis=0)
+ return numpy.linalg.norm(dspline(alpha, self._control_points),
+ axis=0)
self._point_distances = [0.0]
num_alpha = 100
@@ -240,8 +239,8 @@
# might think.
for alpha in numpy.linspace(0.0, 1.0, num_alpha)[1:]:
self._point_distances.append(
- scipy.integrate.fixed_quad(spline_velocity, alpha, alpha + 1.0
- / (num_alpha - 1.0))[0] +
+ scipy.integrate.fixed_quad(spline_velocity, alpha, alpha +
+ 1.0 / (num_alpha - 1.0))[0] +
self._point_distances[-1])
def distance_to_alpha(self, distance):
@@ -256,7 +255,8 @@
if numpy.isscalar(distance):
return numpy.array([self._distance_to_alpha_scalar(distance)])
else:
- return numpy.array([self._distance_to_alpha_scalar(d) for d in distance])
+ return numpy.array(
+ [self._distance_to_alpha_scalar(d) for d in distance])
def _distance_to_alpha_scalar(self, distance):
"""Helper to compute alpha for a distance for a single scalar."""
@@ -264,15 +264,17 @@
return 0.0
elif distance >= self.length():
return 1.0
- after_index = numpy.searchsorted(
- self._point_distances, distance, side='right')
+ after_index = numpy.searchsorted(self._point_distances,
+ distance,
+ side='right')
before_index = after_index - 1
# Linearly interpolate alpha from our (sorted) distance table.
return (distance - self._point_distances[before_index]) / (
self._point_distances[after_index] -
- self._point_distances[before_index]) * (1.0 / (
- len(self._point_distances) - 1.0)) + float(before_index) / (
+ self._point_distances[before_index]) * (
+ 1.0 /
+ (len(self._point_distances) - 1.0)) + float(before_index) / (
len(self._point_distances) - 1.0)
def length(self):
@@ -287,8 +289,8 @@
# TODO(austin): need a better name...
def dxy(self, distance):
"""Returns the xy velocity as a function of distance."""
- dspline_point = dspline(
- self.distance_to_alpha(distance), self._control_points)
+ dspline_point = dspline(self.distance_to_alpha(distance),
+ self._control_points)
return dspline_point / numpy.linalg.norm(dspline_point, axis=0)
# TODO(austin): need a better name...
@@ -298,8 +300,7 @@
dspline_points = dspline(alpha, self._control_points)
ddspline_points = ddspline(alpha, self._control_points)
- norm = numpy.linalg.norm(
- dspline_points, axis=0)**2.0
+ norm = numpy.linalg.norm(dspline_points, axis=0)**2.0
return ddspline_points / norm - numpy.multiply(
dspline_points, (numpy.array(dspline_points)[0, :] *
@@ -309,10 +310,9 @@
def theta(self, distance, dspline_points=None):
"""Returns the heading as a function of distance."""
- return spline_theta(
- self.distance_to_alpha(distance),
- self._control_points,
- dspline_points=dspline_points)
+ return spline_theta(self.distance_to_alpha(distance),
+ self._control_points,
+ dspline_points=dspline_points)
def dtheta(self, distance, dspline_points=None, ddspline_points=None):
"""Returns the angular velocity as a function of distance."""
@@ -327,9 +327,14 @@
return dtheta_points / numpy.linalg.norm(dspline_points, axis=0)
- def dtheta_dt(self, distance, velocity, dspline_points=None, ddspline_points=None):
+ def dtheta_dt(self,
+ distance,
+ velocity,
+ dspline_points=None,
+ ddspline_points=None):
"""Returns the angular velocity as a function of time."""
- return self.dtheta(distance, dspline_points, ddspline_points) * velocity
+ return self.dtheta(distance, dspline_points,
+ ddspline_points) * velocity
def ddtheta(self,
distance,
@@ -400,8 +405,8 @@
return 0.0
return (f(t, y) - a0) / y
- return (RungeKutta(integrablef, v, x, dx) - v
- ) + numpy.sqrt(2.0 * a0 * dx + v * v)
+ return (RungeKutta(integrablef, v, x, dx) - v) + numpy.sqrt(2.0 * a0 * dx +
+ v * v)
class Trajectory(object):
@@ -409,8 +414,8 @@
distance_count):
self._path = path
self._drivetrain = drivetrain
- self.distances = numpy.linspace(0.0,
- self._path.length(), distance_count)
+ self.distances = numpy.linspace(0.0, self._path.length(),
+ distance_count)
self._longitudal_accel = longitudal_accel
self._lateral_accel = lateral_accel
@@ -460,9 +465,10 @@
C = K3 * v * v + K4 * v
# Note: K345 are not quite constant over the step, but we are going
# to assume they are for now.
- accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
- K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
- (-12.0 - C[1, 0]) / K5[1, 0]]
+ accelerations = [
+ (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+ (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+ ]
maxa = -float('inf')
for a in accelerations:
U = K5 * a + K3 * v * v + K4 * v
@@ -473,8 +479,8 @@
# Constrain the longitudinal acceleration to keep in a pseudo friction
# circle. This will make it so we don't floor it while in a turn and
# cause extra wheel slip.
- long_accel = numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
- 2.0) * self._longitudal_accel
+ long_accel = numpy.sqrt(1.0 - (
+ lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
return min(long_accel, maxa)
def forward_pass(self, plan):
@@ -503,9 +509,10 @@
C = K3 * v * v + K4 * v
# Note: K345 are not quite constant over the step, but we are going
# to assume they are for now.
- accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
- K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
- (-12.0 - C[1, 0]) / K5[1, 0]]
+ accelerations = [
+ (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+ (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+ ]
mina = float('inf')
for a in accelerations:
U = K5 * a + K3 * v * v + K4 * v
@@ -516,8 +523,8 @@
# Constrain the longitudinal acceleration to keep in a pseudo friction
# circle. This will make it so we don't floor it while in a turn and
# cause extra wheel slip.
- long_accel = -numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
- 2.0) * self._longitudal_accel
+ long_accel = -numpy.sqrt(1.0 - (
+ lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
return max(long_accel, mina)
def backward_pass(self, plan):
@@ -546,8 +553,9 @@
if distance > self.distances[-1]:
distance = self.distances[-1]
else:
- after_index = numpy.searchsorted(
- self.distances, distance, side='right')
+ after_index = numpy.searchsorted(self.distances,
+ distance,
+ side='right')
before_index = after_index - 1
vforwards = integrate_accel_for_distance(
@@ -586,14 +594,15 @@
return U
def goal_state(self, distance, velocity):
- width = (
- self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
+ width = (self._drivetrain.robot_radius_l +
+ self._drivetrain.robot_radius_r)
goal = numpy.matrix(numpy.zeros((5, 1)))
goal[0:2, :] = self._path.xy(distance)
goal[2, :] = self._path.theta(distance)
- Ter = numpy.linalg.inv(numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
+ Ter = numpy.linalg.inv(
+ numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
goal[3:5, :] = Ter * numpy.matrix(
[[velocity], [self._path.dtheta_dt(distance, velocity)]])
return goal
@@ -618,25 +627,23 @@
# Compute theta and the two derivatives
theta = spline_theta(alphas, control_points, dspline_points=dspline_points)
- dtheta = dspline_theta(
- alphas, control_points, dspline_points=dspline_points)
- ddtheta = ddspline_theta(
- alphas,
- control_points,
- dspline_points=dspline_points,
- dddspline_points=dddspline_points)
+ dtheta = dspline_theta(alphas,
+ control_points,
+ dspline_points=dspline_points)
+ ddtheta = ddspline_theta(alphas,
+ control_points,
+ dspline_points=dspline_points,
+ dddspline_points=dddspline_points)
# Plot the control points and the spline.
pylab.figure()
- pylab.plot(
- numpy.array(control_points)[0, :],
- numpy.array(control_points)[1, :],
- '-o',
- label='control')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(numpy.array(control_points)[0, :],
+ numpy.array(control_points)[1, :],
+ '-o',
+ label='control')
+ pylab.plot(numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
# For grins, confirm that the double integral of the acceleration (with
@@ -657,9 +664,9 @@
# Integrate up the spline velocity and heading to confirm that given a
# velocity (as a function of the spline parameter) and angle, we will move
# from the starting point to the ending point.
- thetaint_plot = numpy.zeros((len(alphas),))
+ thetaint_plot = numpy.zeros((len(alphas), ))
thetaint = theta[0]
- dthetaint_plot = numpy.zeros((len(alphas),))
+ dthetaint_plot = numpy.zeros((len(alphas), ))
dthetaint = dtheta[0]
thetaint_plot[0] = thetaint
dthetaint_plot[0] = dthetaint
@@ -670,15 +677,14 @@
for i in range(len(alphas) - 1):
dalpha = alphas[i + 1] - alphas[i]
txint += dalpha * numpy.linalg.norm(
- dspline_points[:, i]) * numpy.matrix(
- [[numpy.cos(theta[i])], [numpy.sin(theta[i])]])
+ dspline_points[:, i]) * numpy.matrix([[numpy.cos(theta[i])],
+ [numpy.sin(theta[i])]])
txint_plot[:, i + 1] = txint
thetaint += dalpha * dtheta[i]
dthetaint += dalpha * ddtheta[i]
thetaint_plot[i + 1] = thetaint
dthetaint_plot[i + 1] = dthetaint
-
# Now plot x, dx/dalpha, ddx/ddalpha, dddx/dddalpha, and integrals thereof
# to perform consistency checks.
pylab.figure()
@@ -709,11 +715,9 @@
pylab.plot(alphas, ddtheta, label='ddtheta')
pylab.plot(alphas, thetaint_plot, label='thetai')
pylab.plot(alphas, dthetaint_plot, label='dthetai')
- pylab.plot(
- alphas,
- numpy.linalg.norm(
- numpy.array(dspline_points), axis=0),
- label='velocity')
+ pylab.plot(alphas,
+ numpy.linalg.norm(numpy.array(dspline_points), axis=0),
+ label='velocity')
# Now, repeat as a function of path length as opposed to alpha
path = Path(control_points)
@@ -780,12 +784,11 @@
longitudal_accel = 3.0
lateral_accel = 2.0
- trajectory = Trajectory(
- path,
- drivetrain=velocity_drivetrain,
- longitudal_accel=longitudal_accel,
- lateral_accel=lateral_accel,
- distance_count=500)
+ trajectory = Trajectory(path,
+ drivetrain=velocity_drivetrain,
+ longitudal_accel=longitudal_accel,
+ lateral_accel=lateral_accel,
+ distance_count=500)
vmax = numpy.inf
vmax = 10.0
@@ -827,14 +830,16 @@
while spline_state[0, 0] < path.length():
t += dt
+
def along_spline_diffeq(t, x):
_, v, a = trajectory.ff_accel(backward_accel_plan, x[0, 0])
return numpy.matrix([[x[1, 0]], [a]])
- spline_state = RungeKutta(along_spline_diffeq,
- spline_state.copy(), t, dt)
+ spline_state = RungeKutta(along_spline_diffeq, spline_state.copy(), t,
+ dt)
- d, v, a = trajectory.ff_accel(backward_accel_plan, length_plan_x[-1][0, 0])
+ d, v, a = trajectory.ff_accel(backward_accel_plan,
+ length_plan_x[-1][0, 0])
length_plan_v.append(v)
length_plan_a.append(a)
@@ -877,9 +882,10 @@
[path.dtheta_dt(xva_plan[0, i], xva_plan[1, i])[0]]])
vel_lr = numpy.linalg.inv(Ter) * vel_omega_r
- state_plan[:, i] = numpy.matrix(
- [[x_r], [y_r], [theta_r], [vel_lr[0, 0]], [vel_lr[1, 0]]])
- u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i])
+ state_plan[:, i] = numpy.matrix([[x_r], [y_r], [theta_r],
+ [vel_lr[0, 0]], [vel_lr[1, 0]]])
+ u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0,
+ i])
Q = numpy.matrix(
numpy.diag([
@@ -906,16 +912,17 @@
linear_velocity = -kMinVelocity
width = (velocity_drivetrain.robot_radius_l +
- velocity_drivetrain.robot_radius_r)
- A_linearized_continuous = numpy.matrix([[
- 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta, 0.5 *
- costheta
- ], [
- 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta, 0.5 *
- sintheta
- ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0]])
+ velocity_drivetrain.robot_radius_r)
+ A_linearized_continuous = numpy.matrix(
+ [[
+ 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta,
+ 0.5 * costheta
+ ],
+ [
+ 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta,
+ 0.5 * sintheta
+ ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
+ [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]])
A_linearized_continuous[3:5, 3:5] = velocity_drivetrain.A_continuous
B_linearized_continuous = numpy.matrix(numpy.zeros((5, 2)))
B_linearized_continuous[3:5, :] = velocity_drivetrain.B_continuous
@@ -942,15 +949,15 @@
velocity_drivetrain.robot_radius_r)
accel = (velocity_drivetrain.A_continuous * velocity +
velocity_drivetrain.B_continuous * U)
- return numpy.matrix(
- [[numpy.cos(theta) * linear_velocity],
- [numpy.sin(theta) * linear_velocity], [angular_velocity],
- [accel[0, 0]], [accel[1, 0]]])
+ return numpy.matrix([[numpy.cos(theta) * linear_velocity],
+ [numpy.sin(theta) * linear_velocity],
+ [angular_velocity], [accel[0, 0]],
+ [accel[1, 0]]])
full_us[:, i] = U
- state = RungeKutta(lambda t, x: spline_diffeq(U, t, x),
- state, i * dt, dt)
+ state = RungeKutta(lambda t, x: spline_diffeq(U, t, x), state, i * dt,
+ dt)
pylab.figure()
pylab.plot(length_plan_t, numpy.array(xva_plan)[0, :], label='x')
@@ -963,17 +970,14 @@
pylab.legend()
pylab.figure()
- pylab.plot(
- numpy.array(states)[0, :],
- numpy.array(states)[1, :],
- label='robot')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(numpy.array(states)[0, :],
+ numpy.array(states)[1, :],
+ label='robot')
+ pylab.plot(numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
-
def a(_, x):
return 2.0
return 2.0 + 0.0001 * x
diff --git a/frc971/control_loops/python/spline_drawing.py b/frc971/control_loops/python/spline_drawing.py
new file mode 100644
index 0000000..4e8b04b
--- /dev/null
+++ b/frc971/control_loops/python/spline_drawing.py
@@ -0,0 +1,292 @@
+from color import Color, palette
+import cairo
+
+import numpy as np
+from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+PIXELS_ON_SCREEN = 800
+
+X_TRANSLATE = 0
+Y_TRANSLATE = 0
+
+
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
+
+
+def mToPx(m):
+ return (m * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
+
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+ """Draws a cross with fixed dimensions in pixel space."""
+ set_color(cr, color)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+ set_color(cr, palette["LIGHT_GREY"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ set_color(cr, color)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+ set_color(cr, palette["LIGHT_GREY"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+ cr.scale(widtha, -heighta)
+ cr.show_text(text)
+ cr.scale(widthb, -heightb)
+
+
+def redraw(needs_redraw, window):
+ print("Redrew")
+ if not needs_redraw:
+ window.queue_draw()
+
+
+def draw_HAB(cr):
+ print("WENT IN")
+ # BASE Constants
+ X_BASE = 0 + X_TRANSLATE #(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+ BACKWALL_X = X_TRANSLATE
+ LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+ # HAB Levels 2 and 3 called in variables backhab
+ # draw loading stations
+ cr.move_to(0, LOADING_Y)
+ cr.line_to(mToPx(0.6), LOADING_Y)
+ cr.move_to(0, -1.0 * LOADING_Y)
+ cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+
+ BACKWALL_X = X_TRANSLATE
+
+ # HAB Levels 2 and 3 called in variables backhab
+
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ #draw 0, 0
+ set_color(cr, palette["BLACK"])
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(8.2296 / 2.0))
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(-8.2296 / 2.0))
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE + mToPx(8.2296), Y_TRANSLATE)
+
+ cr.stroke()
+
+
+def draw_rockets(cr):
+ # BASE Constants
+ X_BASE = 0 + X_TRANSLATE + mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+ near_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.4))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+ far_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+ print("FAR SIDE ROCKET")
+ #print(far_side_rocket_center)
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+ near_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.4))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+ far_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+ X_BASE = 0 + X_TRANSLATE #mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+
+def draw_cargo_ship(cr):
+ # BASE Constants
+
+ X_BASE = X_TRANSLATE + mToPx(5.59435)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ FRONT_PEG_DELTA_Y = mToPx(0.276352)
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.4), Y_BASE + FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.4), Y_BASE - FRONT_PEG_DELTA_Y)
+
+ SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+ SIDE_PEG_X = X_BASE + mToPx(1.148842)
+ SIDE_PEG_DX = mToPx(0.55245)
+
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+ mToPx(1.41605))
+
+ X_BASE = X_TRANSLATE + mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
+ mToPx(2.43205), mToPx(1.41605))
+
+ cr.stroke()
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
new file mode 100755
index 0000000..2fd7964
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph.py
@@ -0,0 +1,100 @@
+#!/usr/bin/python3
+import gi
+from path_edit import *
+import numpy as np
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk, Gtk, GLib
+import cairo
+
+class GridWindow(Gtk.Window):
+ def method_connect(self, event, cb):
+ def handler(obj, *args):
+ cb(*args)
+
+ print("Method_connect ran")
+ self.connect(event, handler)
+
+ def mouse_move(self, event):
+ #Changes event.x and event.y to be relative to the center.
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.mouse_move(event)
+ self.queue_draw()
+
+ def button_press(self, event):
+ print("button press activated")
+ o_x = event.x
+ o_y = event.y
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = 2 * self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.do_button_press(event)
+ event.x = o_x
+ event.y = o_y
+
+ def key_press(self, event):
+ print("key press activated")
+ self.drawing_area.do_key_press(event, self.file_name_box.get_text())
+ self.queue_draw()
+
+ def configure(self, event):
+ print("configure activated")
+ self.drawing_area.window_shape = (event.width, event.height)
+
+ # handle submitting a constraint
+ def on_submit_click(self, widget):
+ self.drawing_area.inConstraint = int(self.constraint_box.get_text())
+ self.drawing_area.inValue = int(self.value_box.get_text())
+
+ def __init__(self):
+ Gtk.Window.__init__(self)
+
+ self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
+
+ flowBox = Gtk.FlowBox()
+ flowBox.set_valign(Gtk.Align.START)
+ flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
+
+ flowBox.set_valign(Gtk.Align.START)
+
+ self.add(flowBox)
+
+ container = Gtk.Fixed()
+ flowBox.add(container)
+
+ self.eventBox = Gtk.EventBox()
+ container.add(self.eventBox)
+
+ self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+ | Gdk.EventMask.BUTTON_RELEASE_MASK
+ | Gdk.EventMask.POINTER_MOTION_MASK
+ | Gdk.EventMask.SCROLL_MASK
+ | Gdk.EventMask.KEY_PRESS_MASK)
+
+ #add the graph box
+ self.drawing_area = GTK_Widget()
+ self.eventBox.add(self.drawing_area)
+
+ self.method_connect("key-release-event", self.key_press)
+ self.method_connect("button-release-event", self.button_press)
+ self.method_connect("configure-event", self.configure)
+ self.method_connect("motion_notify_event", self.mouse_move)
+
+ self.file_name_box = Gtk.Entry()
+ self.file_name_box.set_size_request(200, 40)
+
+ self.file_name_box.set_text("File")
+ self.file_name_box.set_editable(True)
+
+ container.put(self.file_name_box, 0, 0)
+
+ self.show_all()
+
+
+window = GridWindow()
+RunApp()
diff --git a/frc971/control_loops/python/spline_graph_usage.txt b/frc971/control_loops/python/spline_graph_usage.txt
new file mode 100644
index 0000000..a698298
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph_usage.txt
@@ -0,0 +1,22 @@
+Spline Graph Usage
+
+Run with: bazel run spline_graph -c opt "YOUR SCREEN SIZE"
+
+On Start Place down first 6 points
+Then you go into editing mode, where you can drag your points around
+Then to create another spline, press p and the first three points will be autogenerated
+
+To export type in your file in the top right textfield ex.test.json
+It will be exported to spline_jsons/YOURFILE
+
+Notes:
+If importing gi, gi requires #!/usr/bin/python3 be at the top of the file
+
+This was built on top of BaseWindow
+
+Known Bugs:
+libspline throws errors about viable friction limits and viable voltage limits not overlapping
+bazel run lib_spline_test -c opt
+John commented those lines out
+
+Upon closing, reopens window
diff --git a/frc971/control_loops/python/spline_jsons/test.json b/frc971/control_loops/python/spline_jsons/test.json
new file mode 100644
index 0000000..09737e0
--- /dev/null
+++ b/frc971/control_loops/python/spline_jsons/test.json
@@ -0,0 +1 @@
+[[[1.6680953371273437, -3.4989921261100347], [1.8841223685193285, -3.3960504631707944], [2.0281410171518672, -3.272708402385707], [2.192647314562578, -3.1286520914802223], [2.3984302077132025, -3.0360669258966317], [2.5936689198479943, -2.912724865111545]], [[2.5936689198479943, -2.912724865111545], [2.788907631982786, -2.789382804326458], [2.973602163101745, -2.635283848339876], [3.190230535451773, -2.5732992795250538], [3.396013428602397, -2.511627935280236], [3.6117404172199103, -2.4705139150185405]]]
\ No newline at end of file
diff --git a/frc971/control_loops/python/spline_writer.py b/frc971/control_loops/python/spline_writer.py
new file mode 100644
index 0000000..df622f3
--- /dev/null
+++ b/frc971/control_loops/python/spline_writer.py
@@ -0,0 +1,72 @@
+import os
+import numpy as np
+
+
+class SplineWriter(object):
+ def __init__(self, namespaces=None, filename="auto_splines.cc"):
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'autonomous']
+
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
+
+ self._namespace_end = '\n'.join([
+ '} // namespace %s' % name for name in reversed(self._namespaces)
+ ])
+
+ self.filename_ = filename
+
+ def make_string_list(self, list):
+ string = "{{"
+ for i in range(0, len(list)):
+ if i == len(list) - 1:
+ string += str(list[i])
+ else:
+ string += str(list[i]) + ", "
+ return string + "}}"
+
+ def Write(self, spline_name, spline_idx, control_points):
+ """Writes the cc file to the file named cc_file."""
+ xs = control_points[:, 0]
+ ys = control_points[:, 1]
+ spline_count = (len(xs) - 6) / 5 + 1
+ with open(self.filename_, 'a') as fd:
+ fd.write(self._namespace_start)
+ #write the name
+ fd.write("\n\n::frc971::MultiSpline " + spline_name + "() {\n")
+ # write the objs, at the moment assumes a single constraint, needs fixing
+ fd.write(
+ "\t::frc971::MultiSpline spline;\n\t::frc971::Constraint constraints;\n"
+ )
+ fd.write(
+ "\tconstraints.constraint_type = 0;\n\tconstraints.value = 0;\n"
+ )
+ fd.write(
+ "\tconstraints.start_distance = 0;\n\tconstraints.end_distance = 0;\n"
+ )
+ fd.write('\n')
+ fd.write("\tspline.spline_idx = " + str(spline_idx) +
+ ";\n\tspline.spline_count = " + str(spline_count) + ";\n")
+ fd.write("\tspline.spline_x = " + self.make_string_list(xs) +
+ ";\n\tspline.spline_y = " + self.make_string_list(ys) +
+ ";\n")
+ fd.write(
+ "\tspline.constraints = {{constraints}};\n\treturn spline;\n")
+ fd.write("}")
+ fd.write("\n\n")
+ fd.write(self._namespace_end)
+ fd.write("\n\n")
+
+
+def main():
+ writer = SplineWriter()
+ points = np.array([[1.0, 2.0], [3.0, 4.0], [5.0, 6.0], [7.0, 8.0],
+ [9.0, 10.0], [11.0, 12.0]])
+ spline_name = "test_spline"
+ spline_idx = 1
+ writer.Write(spline_name, spline_idx, points)
+
+
+main()
diff --git a/y2020/constants.cc b/y2020/constants.cc
index a35798f..90129dd 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -26,7 +26,6 @@
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 9971;
-const uint16_t kCodingRobotTeamNumber = 7971;
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
@@ -120,7 +119,7 @@
turret_params->zeroing_constants.measured_absolute_position = 0.0;
break;
- case kCodingRobotTeamNumber:
+ case Values::kCodingRobotTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
intake->zeroing_constants.measured_absolute_position = 0.0;
diff --git a/y2020/constants.h b/y2020/constants.h
index 39f6e36..7fbb704 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -20,6 +20,8 @@
namespace constants {
struct Values {
+ static const uint16_t kCodingRobotTeamNumber = 7971;
+
static const int kZeroingSampleSize = 200;
static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index a1083eb..b336fd7 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -25,6 +25,7 @@
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/make_unique.h"
+#include "aos/network/team_number.h"
#include "aos/realtime.h"
#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
@@ -504,9 +505,19 @@
// Note: If ADIS16470 is plugged in directly to the roboRIO SPI port without
// the Spartan Board, then trigger is on 26, reset 27, and chip select is
// CS0.
- auto imu_trigger = make_unique<frc::DigitalInput>(0);
- auto imu_reset = make_unique<frc::DigitalOutput>(1);
- auto spi = make_unique<frc::SPI>(frc::SPI::Port::kOnboardCS2);
+ frc::SPI::Port spi_port = frc::SPI::Port::kOnboardCS2;
+ std::unique_ptr<frc::DigitalInput> imu_trigger;
+ std::unique_ptr<frc::DigitalOutput> imu_reset;
+ if (::aos::network::GetTeamNumber() ==
+ constants::Values::kCodingRobotTeamNumber) {
+ imu_trigger = make_unique<frc::DigitalInput>(26);
+ imu_reset = make_unique<frc::DigitalOutput>(27);
+ spi_port = frc::SPI::Port::kOnboardCS0;
+ } else {
+ imu_trigger = make_unique<frc::DigitalInput>(0);
+ imu_reset = make_unique<frc::DigitalOutput>(1);
+ }
+ auto spi = make_unique<frc::SPI>(spi_port);
frc971::wpilib::ADIS16470 imu(&sensor_reader_event_loop, spi.get(),
imu_trigger.get(), imu_reset.get());
sensor_reader.set_imu(&imu);