copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/frc971/input/GyroReader.cc b/frc971/input/GyroReader.cc
new file mode 100644
index 0000000..a6e1d00
--- /dev/null
+++ b/frc971/input/GyroReader.cc
@@ -0,0 +1,50 @@
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "aos/aos_core.h"
+
+#include "frc971/queues/GyroAngle.q.h"
+
+#define M_PI 3.14159265358979323846264338327
+
+using frc971::sensors::gyro;
+
+int main(){
+ aos::Init();
+ int fd = open("/dev/aschuh0", O_RDONLY);
+ int rate_limit = 0;
+ if (fd < 0) {
+ LOG(ERROR, "No Gyro found.\n");
+ } else {
+ LOG(INFO, "Gyro now connected\n");
+ }
+
+ while (true) {
+ int64_t gyro_value;
+ if (read(fd, (void *)&gyro_value, sizeof(gyro_value)) != sizeof(gyro_value)) {
+ LOG(ERROR, "Could not read gyro errno: %d\n", errno);
+ if (errno == ENODEV || errno == EBADF) {
+ close(fd);
+ while (1) {
+ usleep(1000);
+ fd = open("/dev/aschuh0", O_RDONLY);
+ if (fd > 0) {
+ LOG(INFO, "Found gyro again\n");
+ break;
+ }
+ }
+ }
+ continue;
+ }
+ rate_limit ++;
+ if (rate_limit > 10) {
+ LOG(DEBUG, "Gyro is %d\n", (int)(gyro_value / 16));
+ rate_limit = 0;
+ }
+ gyro.MakeWithBuilder().angle(gyro_value / 16.0 / 1000.0 / 180.0 * M_PI).Send();
+ }
+
+ aos::Cleanup();
+}