From 13678345cc8ad74b9ab0c809fda5ebcc6f7dcd01 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Mon, 4 Jul 2022 15:54:03 +0800
Subject: [PATCH] motion-control: add joycon support

---
 cmd/motion-control/main+joycon.ha | 168 ++++++++++++++++++++++++++++++
 1 file changed, 168 insertions(+)
 create mode 100644 cmd/motion-control/main+joycon.ha

diff --git a/cmd/motion-control/main+joycon.ha b/cmd/motion-control/main+joycon.ha
new file mode 100644
index 0000000..d72307e
--- /dev/null
+++ b/cmd/motion-control/main+joycon.ha
@@ -0,0 +1,168 @@
+use ahrs::complementary;
+use errors;
+use fmt;
+use getopt;
+use io;
+use linux::timerfd;
+use math;
+use net;
+use net::ip;
+use net::udp;
+use os;
+use proto::control;
+use rt;
+use sensors::joycon;
+use strconv;
+use time;
+use unix::signal;
+
+type state = enum u8 {
+	NORMAL,
+	BLOCK,
+	RESET,
+	EXIT,
+};
+
+let global_state: state	= state::NORMAL;
+
+fn sigint(sig: int, info: *signal::siginfo, ucontext: *void) void = {
+	global_state = state::EXIT;
+};
+
+fn sigusr1(sig: int, info: *signal::siginfo, ucontext: *void) void = {
+	switch (global_state) {
+	case state::NORMAL =>
+		fmt::println("Block")!;
+		global_state = state::BLOCK;
+	case state::BLOCK =>
+		fmt::println("Reset")!;
+		global_state = state::RESET;
+	case state::RESET =>
+		fmt::println("Reset is already pending")!;
+	case state::EXIT =>
+		fmt::println("Exit pending")!;
+	};
+};
+
+export fn main() int = {
+	const help: []getopt::help = [
+		"control pointerd with motion",
+		"addr port",
+	];
+	const cmd = getopt::parse(os::args, help...);
+	defer getopt::finish(&cmd);
+	if (len(cmd.args) != 2) {
+		getopt::printusage(os::stderr, os::args[0], help);
+		os::exit(-1);
+	};
+	const addr = match (ip::parse(cmd.args[0])) {
+	case let addr: ip::addr =>
+		yield addr;
+	case ip::invalid =>
+		fmt::fatalf("Invalid address {}", cmd.args[0]);
+	};
+	const port = match (strconv::stou16(cmd.args[1])) {
+	case let port: u16 =>
+		yield port;
+	case =>
+		fmt::fatalf("Invalid port {}", cmd.args[1]);
+	};
+
+	const buf = control::request {
+		code = control::code::RELATIVE_MOVEMENT,
+		...,
+	};
+
+	const sock = match (udp::connect(addr, port)) {
+	case let s: net::socket =>
+		yield s;
+	case let err: net::error =>
+		fmt::fatalf("Cannot connect() {}:{}: {}", ip::string(addr), port, net::strerror(err));
+	};
+	defer net::close(sock)!;
+
+	const epoll = rt::epoll_create1(rt::EPOLL_CLOEXEC)!;
+	defer io::close(epoll)!;
+
+	const joycon = joycon::new(void)!;
+	defer joycon::destroy(joycon);
+
+	const joycon_epoll = rt::epoll_event {
+		events = rt::EPOLLIN,
+		data = rt::epoll_data {
+			fd = joycon.fd,
+		},
+	};
+	rt::epoll_ctl(epoll, rt::EPOLL_CTL_ADD, joycon.fd, &joycon_epoll)!;
+
+	const fd = timerfd::new(time::clock::MONOTONIC, rt::O_NONBLOCK)!;
+	defer io::close(fd)!;
+	timerfd::set(fd, (10 * time::MILLISECOND): timerfd::interval, 0)!;
+	const timer_epoll = rt::epoll_event {
+		events = rt::EPOLLIN | rt::EPOLLET,
+		data = rt::epoll_data {
+			fd = fd,
+		},
+	};
+	rt::epoll_ctl(epoll, rt::EPOLL_CTL_ADD, fd, &timer_epoll)!;
+
+	signal::handle(signal::SIGINT, &sigint, signal::flag::RESTART);
+	signal::handle(signal::SIGUSR1, &sigusr1, signal::flag::RESTART);
+
+	const sample = joycon::next_sample(joycon)!;
+	let prev_us = sample.timestamp;
+
+	let complementary = complementary::new(sample.accelerometer, 0.02);
+
+	let e = rt::epoll_event {...};
+	for (global_state != state::EXIT) {
+		match (rt::epoll_wait(epoll, &e, 1, -1)) {
+		case let e: rt::errno =>
+			switch (e) {
+			case rt::EINTR =>
+				break;
+			case =>
+				abort("Unexpected error");
+			};
+		case =>
+			yield;
+		};
+		if (e.data.fd == fd) {
+			timerfd::read(fd)!;
+			switch (global_state) {
+			case state::NORMAL =>
+				let attitude = complementary::read_euler(complementary);
+				buf.x = ((attitude.2 / math::PI * 180.0): int / 8): i8;
+				buf.y = ((-attitude.1 / math::PI * 180.0): int / 4): i8;
+				match (udp::send(sock, buf.bytes)) {
+				case let err: net::error =>
+					fmt::fatalf("Cannot send(): {}", net::strerror(err));
+				case size =>
+					yield;
+				};
+			case state::BLOCK =>
+				yield;
+			case state::RESET =>
+				complementary::destroy(complementary);
+				const sample = joycon::next_sample(joycon)!;
+				complementary = complementary::new(sample.accelerometer, 0.02);
+				global_state = state::NORMAL;
+			case state::EXIT =>
+				break;
+			};
+		} else if (e.data.fd == joycon.fd) {
+			const sample = joycon::next_sample(joycon)!;
+			const gyro_readings = sample.gyroscope;
+			const acc_readings = sample.accelerometer;
+			let rps = (gyro_readings.0 / 180.0 * math::PI,
+				gyro_readings.1 / 180.0 * math::PI,
+				gyro_readings.2 / 180.0 * math::PI);
+			const dt = (sample.timestamp - prev_us): f64 / 1000000.0;
+			complementary::update(complementary, rps, dt);
+			complementary::update_accelerometer(complementary, acc_readings);
+			prev_us = sample.timestamp;
+		};
+	};
+
+	return 0;
+};
-- 
2.43.0