Merge "Add alert for scouting" into main
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 8b97933..239f49a 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -233,10 +233,13 @@
# TODO(austin): Don't penalize torque steering current.
for i in range(4):
+ Is = U[2 * i + 0]
+ Id = U[2 * i + 1]
# Steer
- J += U[2 * i + 0] * U[2 * i + 0] / 100000.0
+ J += ((Is + dynamics.STEER_CURRENT_COUPLING_FACTOR * Id)**
+ 2.0) / 100000.0
# Drive
- J += U[2 * i + 1] * U[2 * i + 1] / 1000.0
+ J += Id * Id / 1000.0
return casadi.Function("Jn", [X, U, R], [J])
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index e223b74..3c4eaf6 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -617,6 +617,16 @@
sub(integer(1), div(rb1_, rp_)))),
div(rw_, rb2_))))))))));
result_py.emplace_back("");
+ result_py.emplace_back("# Is = STEER_CURRENT_COUPLING_FACTOR * Id");
+ result_py.emplace_back(absl::Substitute(
+ "STEER_CURRENT_COUPLING_FACTOR = $0",
+ ccode(*(neg(
+ mul(div(Gs_, Kts_),
+ mul(div(Ktd_, mul(Gd_, rw_)),
+ neg(mul(add(neg(wb_), mul(add(rs_, rp_),
+ sub(integer(1), div(rb1_, rp_)))),
+ div(rw_, rb2_))))))))));
+ result_py.emplace_back("");
result_py.emplace_back("# Returns the derivative of our state vector");
result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 9aa6457..0a3fdb6 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -582,6 +582,23 @@
self.assertLess(xdot[dynamics.STATE_OMEGAD2, 0], -100.0)
self.assertLess(xdot[dynamics.STATE_OMEGAD3, 0], -100.0)
+ def test_steer_coupling(self):
+ """Tests that the steer coupling factor cancels out steer coupling torque."""
+ steer_I = numpy.array(
+ [[dynamics.STEER_CURRENT_COUPLING_FACTOR * 10.0], [10.0]] * 4)
+
+ X = utils.state_vector(
+ velocity=numpy.array([[0.0], [0.0]]),
+ omega=0.0,
+ )
+ X_velocity = self.to_velocity_state(X)
+ Xdot = self.velocity_swerve_physics(X_velocity, steer_I)
+
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS0, 0], 0.0)
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS1, 0], 0.0)
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS2, 0], 0.0)
+ self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS3, 0], 0.0)
+
if __name__ == "__main__":
unittest.main()
diff --git a/frc971/control_loops/swerve/spline_ui/BUILD b/frc971/control_loops/swerve/spline_ui/BUILD
new file mode 100644
index 0000000..8b98959
--- /dev/null
+++ b/frc971/control_loops/swerve/spline_ui/BUILD
@@ -0,0 +1,34 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+aos_downloader_dir(
+ name = "www_files",
+ srcs = [
+ "//frc971/control_loops/swerve/spline_ui/www:static_files",
+ ],
+ dir = "www",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "server",
+ srcs = [
+ "server.cc",
+ ],
+ data = [
+ "//frc971/control_loops/swerve/spline_ui/www:static_files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/containers:ring_buffer",
+ "//aos/events:shm_event_loop",
+ "//aos/logging",
+ "//aos/network:gen_embedded",
+ "//aos/seasocks:seasocks_logger",
+ "//aos/time",
+ "//third_party/seasocks",
+ "@com_google_protobuf//:protobuf",
+ ],
+)
diff --git a/frc971/control_loops/swerve/spline_ui/server.cc b/frc971/control_loops/swerve/spline_ui/server.cc
new file mode 100644
index 0000000..5d8224c
--- /dev/null
+++ b/frc971/control_loops/swerve/spline_ui/server.cc
@@ -0,0 +1,108 @@
+#include "seasocks/Server.h"
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <array>
+#include <cmath>
+#include <memory>
+#include <set>
+#include <sstream>
+
+#include "absl/flags/flag.h"
+#include "google/protobuf/util/json_util.h"
+
+#include "aos/containers/ring_buffer.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/seasocks/seasocks_logger.h"
+#include "aos/time/time.h"
+#include "internal/Embedded.h"
+#include "seasocks/StringUtil.h"
+#include "seasocks/WebSocket.h"
+
+ABSL_FLAG(int, port, 1180, "Port number to serve on");
+
+namespace frc971::control_loops::swerve::spline_ui {
+
+namespace chrono = ::std::chrono;
+
+class WebsocketHandler : public seasocks::WebSocket::Handler {
+ public:
+ WebsocketHandler();
+ void onConnect(seasocks::WebSocket *connection) override;
+ void onData(seasocks::WebSocket *connection, const char *data) override;
+ void onDisconnect(seasocks::WebSocket *connection) override;
+
+ void SendData(const std::string &data);
+
+ private:
+ std::set<seasocks::WebSocket *> connections_;
+};
+
+WebsocketHandler::WebsocketHandler() {}
+
+void WebsocketHandler::onConnect(seasocks::WebSocket *connection) {
+ connections_.insert(connection);
+ AOS_LOG(INFO, "Connected: %s : %s\n", connection->getRequestUri().c_str(),
+ seasocks::formatAddress(connection->getRemoteAddress()).c_str());
+}
+
+void WebsocketHandler::onData(seasocks::WebSocket *connection,
+ const char *data) {
+ // TODO(Nathan): needs tests to check/modify file name
+ AOS_LOG(INFO, "Got data: %s\n", data);
+ connection->send("I got your data!");
+ // parse websocket.send(filePath + '\n' + splineData);
+ // write splineData to filePath
+ std::string_view data_view(data);
+ size_t newline = data_view.find('\n');
+ if (newline == std::string_view::npos) {
+ AOS_LOG(ERROR, "No newline found in data: %s\n", data);
+ return;
+ }
+ std::string_view file_path = data_view.substr(0, newline);
+ std::string_view spline_data = data_view.substr(newline + 1);
+ aos::util::WriteStringToFileOrDie(
+ absl::StrCat(getenv("BUILD_WORKSPACE_DIRECTORY"), file_path),
+ spline_data);
+
+ AOS_LOG(
+ INFO, "Wrote data to file: %s\n",
+ absl::StrCat(getenv("BUILD_WORKSPACE_DIRECTORY"), spline_data).c_str());
+}
+
+void WebsocketHandler::onDisconnect(seasocks::WebSocket *connection) {
+ connections_.erase(connection);
+ AOS_LOG(INFO, "Disconnected: %s : %s\n", connection->getRequestUri().c_str(),
+ seasocks::formatAddress(connection->getRemoteAddress()).c_str());
+}
+
+void WebsocketHandler::SendData(const std::string &data) {
+ for (seasocks::WebSocket *websocket : connections_) {
+ websocket->send(data.c_str());
+ }
+}
+
+} // namespace frc971::control_loops::swerve::spline_ui
+
+int main(int argc, char **argv) {
+ // Make sure to reference this to force the linker to include it.
+ findEmbeddedContent("");
+
+ ::aos::InitGoogle(&argc, &argv);
+
+ seasocks::Server server(::std::shared_ptr<seasocks::Logger>(
+ new ::aos::seasocks::SeasocksLogger(seasocks::Logger::Level::Info)));
+
+ auto websocket_handler = std::make_shared<
+ frc971::control_loops::swerve::spline_ui::WebsocketHandler>();
+ server.addWebSocketHandler("/ws", websocket_handler);
+
+ server.serve("frc971/control_loops/swerve/spline_ui/www/static_files",
+ absl::GetFlag(FLAGS_port));
+
+ return 0;
+}
diff --git a/frc971/control_loops/swerve/spline_ui/www/app/app.module.ts b/frc971/control_loops/swerve/spline_ui/www/app/app.module.ts
index 6948041..6e5396b 100644
--- a/frc971/control_loops/swerve/spline_ui/www/app/app.module.ts
+++ b/frc971/control_loops/swerve/spline_ui/www/app/app.module.ts
@@ -1,7 +1,7 @@
import {NgModule} from '@angular/core';
import {BrowserModule} from '@angular/platform-browser';
import {BrowserAnimationsModule} from '@angular/platform-browser/animations';
-
+import {FormsModule} from '@angular/forms';
import {App} from './app';
@NgModule({
@@ -9,6 +9,7 @@
imports: [
BrowserModule,
BrowserAnimationsModule,
+ FormsModule,
],
exports: [App],
bootstrap: [App],
diff --git a/frc971/control_loops/swerve/spline_ui/www/app/app.ng.html b/frc971/control_loops/swerve/spline_ui/www/app/app.ng.html
index 6e43eac..dc753d3 100644
--- a/frc971/control_loops/swerve/spline_ui/www/app/app.ng.html
+++ b/frc971/control_loops/swerve/spline_ui/www/app/app.ng.html
@@ -1 +1,11 @@
<h1>Spline UI</h1>
+<input type="text" [(ngModel)]="fileName" placeholder="File Name">
+<button (click)="save()">Save</button>
+
+<select [(ngModel)]="year">
+ <option value="2020">2020</option>
+ <option value="2021">2021</option>
+ <option value="2022">2022</option>
+ <option value="2023">2023</option>
+ <option value="2024">2024</option>
+</select>
\ No newline at end of file
diff --git a/frc971/control_loops/swerve/spline_ui/www/app/app.ts b/frc971/control_loops/swerve/spline_ui/www/app/app.ts
index dc0cb7c..fd4e82f 100644
--- a/frc971/control_loops/swerve/spline_ui/www/app/app.ts
+++ b/frc971/control_loops/swerve/spline_ui/www/app/app.ts
@@ -6,4 +6,58 @@
styleUrls: ['../app/common.css'],
})
export class App {
-}
+ fileName: string = 'spline.json';
+ year: number = 2024;
+ splineCount: number = 0;
+ splineX: number[] = [0, 0, 0, 0, 0, 0];
+ splineY: number[] = [0, 0, 0, 0, 0, 0];
+ longConstraint: number = 10;
+ latConstraint: number = 10;
+ voltageConstraint: number = 10;
+
+ save() {
+ //make a websocket call to save the spline
+ const websocket = new WebSocket('ws://localhost:1180/ws');
+ websocket.addEventListener('open', () => {
+ const splineJson = {
+ spline_count: this.splineCount,
+ spline_x: this.splineX,
+ spline_y: this.splineY,
+ constraints: [
+ {
+ constraint_type: "LONGITUDINAL_ACCELERATION",
+ value: this.longConstraint
+ },
+ {
+ constraint_type: "LATERAL_ACCELERATION",
+ value: this.latConstraint
+ },
+ {
+ constraint_type: "VOLTAGE",
+ value: this.voltageConstraint
+ }
+ ]
+ };
+ const splineData = JSON.stringify(splineJson, null, 4);
+ const filePath = this.getPath(this.year) + this.fileName;
+ websocket.send(filePath + '\n' + splineData);
+ });
+ }
+
+ // copied from //frc971/control_loops/python/constants.py
+ // returns the path to the spline jsons for the given year
+ getPath(year: number): string {
+ if(year == 2020 || year == 2021) {
+ return "/y2020/actors/splines/";
+ }
+ else if(year == 2022) {
+ return "/y2022/actors/splines/";
+ }
+ else if(year == 2023 || year == 2024) {
+ return "/y" + year + "/autonomous/splines/";
+ }
+ else {
+ return "/frc971/control_loops/python/spline_jsons/";
+ }
+ }
+}
\ No newline at end of file
diff --git a/frc971/control_loops/swerve/spline_ui/www/package.json b/frc971/control_loops/swerve/spline_ui/www/package.json
index 54a7f09..58ce814 100644
--- a/frc971/control_loops/swerve/spline_ui/www/package.json
+++ b/frc971/control_loops/swerve/spline_ui/www/package.json
@@ -1,6 +1,8 @@
{
+ "name": "@org_frc971/control_loops/swerve/spline_ui/www",
"private": true,
"dependencies": {
- "@angular/animations": "v16-lts"
+ "@angular/animations": "v16-lts",
+ "@angular/forms": "v16-lts"
}
}
diff --git a/pnpm-lock.yaml b/pnpm-lock.yaml
index c8e5425..807e87b 100644
--- a/pnpm-lock.yaml
+++ b/pnpm-lock.yaml
@@ -175,6 +175,9 @@
'@angular/animations':
specifier: v16-lts
version: 16.2.12(@angular/core@16.2.12)
+ '@angular/forms':
+ specifier: v16-lts
+ version: 16.2.12(@angular/common@16.2.12)(@angular/core@16.2.12)(@angular/platform-browser@16.2.12)(rxjs@7.5.7)
scouting/webserver/requests/messages: {}