~xdavidwu/motion-control

1f9ec889053760090c7d0b5053f05d7cdfb54379 — xdavidwu 2 years ago 1c5252b
add controlling client for gy-801
3 files changed, 119 insertions(+), 1 deletions(-)

M .gitignore
M Makefile
A cmd/motion-control/main.ha
M .gitignore => .gitignore +1 -0
@@ 3,3 3,4 @@
/gy-801-dump
/pointerc
/pointerd
/motion-control

M Makefile => Makefile +3 -1
@@ 4,4 4,6 @@ gy-801-dump:
	hare build -o $@ tools/$@/
pointerd:
	hare build -levdev -o $@ cmd/$@/
.PHONY: evdev-dump-events uinput-pointer gy-801-dump pointerd pointerc
motion-control:
	hare build -o $@ cmd/$@/
.PHONY: evdev-dump-events uinput-pointer gy-801-dump pointerd pointerc motion-control

A cmd/motion-control/main.ha => cmd/motion-control/main.ha +115 -0
@@ 0,0 1,115 @@
use ahrs::complementary;
use fmt;
use fs;
use io;
use linux::timerfd;
use math;
use net;
use net::ip;
use net::udp;
use os;
use proto::control;
use rt;
use sensors::ADXL345;
use sensors::L3G4200D;
use strconv;
use time;
use unix::signal;

let exit: bool	= false;

fn sigint(sig: int, info: *signal::siginfo, ucontext: *void) void = {
	exit = true;
};

export fn main() int = {
	if (len(os::args) != 4) {
		fmt::fatalf("Usage: {} bus addr port", os::args[0]);
	};
	const bus = match (strconv::stoi(os::args[1])) {
	case let bus: int =>
		yield bus;
	case =>
		fmt::fatal("bus should be a integer");
	};
	const addr = match (ip::parse(os::args[2])) {
	case let addr: ip::addr =>
		yield addr;
	case ip::invalid =>
		fmt::fatalf("Invalid address {}", os::args[1]);
	};
	const port = match (strconv::stou16(os::args[3])) {
	case let port: u16 =>
		yield port;
	case =>
		fmt::fatalf("Invalid port {}", os::args[2]);
	};

	const buf = control::request {
		code = control::code::RELATIVE_MOVEMENT,
		...,
	};

	const sock = match (udp::connect(addr, port)) {
	case let s: io::file =>
		yield s;
	case let err: net::error =>
		fmt::fatalf("Cannot connect() {}:{}: {}", ip::string(addr), port, net::strerror(err));
	};
	defer io::close(sock)!;

	const ADXL345 = match (ADXL345::new(bus)) {
	case let ADXL345: ADXL345::ADXL345 =>
		yield ADXL345;
	case let err: fs::error =>
		fmt::fatalf("Cannot open i2c device: {}", fs::strerror(err));
	case let err: rt::errno =>
		fmt::fatalf("Cannot ioctl i2c device: {}", rt::strerror(err));
	};
	defer ADXL345::destroy(ADXL345);

	const L3G4200D = match (L3G4200D::new(bus)) {
	case let L3G4200D: L3G4200D::L3G4200D =>
		yield L3G4200D;
	case let err: fs::error =>
		fmt::fatalf("Cannot open i2c device: {}", fs::strerror(err));
	case let err: rt::errno =>
		fmt::fatalf("Cannot ioctl i2c device: {}", rt::strerror(err));
	};
	defer L3G4200D::destroy(L3G4200D);

	ADXL345::init(ADXL345)!;
	L3G4200D::init(L3G4200D)!;

	const fd = timerfd::new(time::clock::MONOTONIC, 0)!;
	defer io::close(fd)!;
	timerfd::set(fd, (10 * time::MILLISECOND): timerfd::interval, 0)!;
	signal::handle(signal::SIGINT, &sigint);

	timerfd::read(fd)!;
	const acc_readings = ADXL345::read(ADXL345)!;
	const complementary = complementary::new(acc_readings, 0.02);

	for (let i = 0; !exit; i += 1) {
		timerfd::read(fd)!;
		const gyro_readings = L3G4200D::read(L3G4200D)!;
		const acc_readings = ADXL345::read(ADXL345)!;
		let rps = (gyro_readings.0 / 180.0 * math::PI,
			gyro_readings.1 / 180.0 * math::PI,
			gyro_readings.2 / 180.0 * math::PI);
		complementary::update(complementary, rps, 0.01);
		complementary::update_accelerometer(complementary, acc_readings);

		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 / 8): i8;
		match (udp::send(sock, buf.bytes)) {
		case let err: net::error =>
			fmt::fatalf("Cannot send(): {}", net::strerror(err));
		case size =>
			yield;
		};
	};

	return 0;
};