@@ 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
@@ 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;
+};