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/AutoMode.act b/frc971/input/AutoMode.act
new file mode 100644
index 0000000..b7b98c3
--- /dev/null
+++ b/frc971/input/AutoMode.act
@@ -0,0 +1,7 @@
+args automode_args {
+};
+status automode_status {
+};
+//has OnEnd;
+//has OnStart;
+priority 99;
diff --git a/frc971/input/AutoMode.cc b/frc971/input/AutoMode.cc
new file mode 100644
index 0000000..254d6b6
--- /dev/null
+++ b/frc971/input/AutoMode.cc
@@ -0,0 +1,15 @@
+#include "frc971/input/AutoMode.q.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include "aos/common/control_loop/Timing.h"
+
+namespace frc971 {
+
+void AutoMode_t::DoAction() {
+ Sleep(10); // wait until it ends
+}
+
+} // namespace frc971
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();
+}
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
new file mode 100644
index 0000000..e0cfbac
--- /dev/null
+++ b/frc971/input/JoystickReader.cc
@@ -0,0 +1,100 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/input/FRCComm.h"
+#include "aos/atom_code/input/JoystickInput.h"
+
+#include "frc971/input/AutoMode.q.h"
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+using ::frc971::sensors::gyro;
+
+namespace frc971 {
+
+class JoystickReader : public aos::JoystickInput {
+ public:
+ JoystickReader() : aos::JoystickInput() {
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+
+ virtual void RunIteration() {
+ static bool is_high_gear = false;
+
+ if (Pressed(0, AUTONOMOUS)) {
+ if (PosEdge(0, ENABLED)){
+ LOG(INFO, "Starting auto mode\n");
+ AutoMode.Start();
+ }
+ if (NegEdge(0, ENABLED)) {
+ LOG(INFO, "Stopping auto mode\n");
+ AutoMode.Stop();
+ }
+ } else { // teleop
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = control_data_.stick0Axis1 / 127.0;
+ const double throttle = -control_data_.stick1Axis2 / 127.0;
+ const double kThrottleGain = 1.0 / 2.5;
+ if (Pressed(0, 7) || Pressed(0, 11)) {
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (PosEdge(0, 7) || PosEdge(0, 11)) {
+ if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) / 2.0
+ - throttle * kThrottleGain / 2.0;
+ angle = gyro->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ //const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance < -kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!(drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear).quickturn(Pressed(0, 5))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal).right_goal(right_goal).Send())) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+
+ if (PosEdge(1, 1)) {
+ is_high_gear = false;
+ }
+ if (PosEdge(1, 3)) {
+ is_high_gear = true;
+ }
+ }
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::JoystickReader)
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
new file mode 100644
index 0000000..fd14df4
--- /dev/null
+++ b/frc971/input/SensorReader.cc
@@ -0,0 +1,44 @@
+#define __STDC_LIMIT_MACROS
+
+#include <arpa/inet.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/input/SensorInput.h"
+
+#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/queues/sensor_values.h"
+
+#define M_PI 3.14159265358979323846
+
+using ::frc971::control_loops::drivetrain;
+
+namespace frc971 {
+
+namespace {
+inline double drivetrain_translate(int32_t in) {
+ // TODO(2013) fix the math
+ return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
+ (3.5 * 2.54 / 100.0 * M_PI);
+}
+} // namespace
+
+class SensorReader : public aos::SensorInput<sensor_values> {
+ virtual void RunIteration(sensor_values &sensors) {
+ for (size_t i = 0; i < sizeof(sensors.encoders) / sizeof(sensors.encoders[0]); ++i) {
+ sensors.encoders[i] = ntohl(sensors.encoders[i]);
+ }
+
+ // TODO(aschuh): Convert to meters.
+ const double left_encoder = drivetrain_translate(sensors.lencoder);
+ const double right_encoder = drivetrain_translate(sensors.rencoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::SensorReader)
diff --git a/frc971/input/SensorWriter.cc b/frc971/input/SensorWriter.cc
new file mode 100644
index 0000000..1fb34db
--- /dev/null
+++ b/frc971/input/SensorWriter.cc
@@ -0,0 +1,40 @@
+#include <arpa/inet.h>
+
+#include "WPILib/Task.h"
+#include "WPILib/Encoder.h"
+#include "WPILib/DigitalInput.h"
+#include "WPILib/Counter.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/motor_server/SensorOutput.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/mutex.h"
+#include "aos/crio/shared_libs/interrupt_notifier.h"
+
+#include "frc971/queues/sensor_values.h"
+
+using ::aos::MutexLocker;
+
+namespace frc971 {
+
+class SensorWriter : public aos::SensorOutput<sensor_values> {
+ Encoder lencoder;
+ Encoder rencoder;
+
+ public:
+ SensorWriter() : lencoder(1, 2), rencoder(3, 4) {
+ lencoder.Start();
+ rencoder.Start();
+
+ printf("frc971::SensorWriter started\n");
+ }
+
+ virtual void RunIteration(sensor_values &vals) {
+ vals.lencoder = htonl(-lencoder.GetRaw());
+ vals.rencoder = -htonl(-rencoder.GetRaw());
+ }
+};
+
+} // namespace frc971
+
+AOS_RUN(frc971::SensorWriter)
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
new file mode 100644
index 0000000..43f591a
--- /dev/null
+++ b/frc971/input/input.gyp
@@ -0,0 +1,80 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'actions',
+ 'type': 'static_library',
+ 'sources': ['AutoMode.act'],
+ 'variables': {
+ 'header_path': 'frc971/input',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'JoystickReader',
+ 'type': 'executable',
+ 'sources': [
+ 'JoystickReader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/atom_code/input/input.gyp:joystick',
+ '<(AOS)/common/network/network.gyp:socket',
+ 'actions',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'SensorReader',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'SensorReader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/network/network.gyp:socket',
+ ],
+ },
+ {
+ 'target_name': 'SensorWriter',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'SensorWriter.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ ],
+ },
+ {
+ 'target_name': 'GyroReader',
+ 'type': 'executable',
+ 'sources': [
+ 'GyroReader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ ],
+ },
+ {
+ 'target_name': 'AutoMode',
+ 'type': 'executable',
+ 'sources': [
+ 'AutoMode.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ 'actions',
+ ],
+ },
+ ],
+}