started cleanup up the socket mess
removed unused #include + dependency
more formatting fixes + fixed users of ReceiveSocket
cleaned more stuff up (converted from references to pointers is one)
wip. started rewriting everything, not quite finished
got everything except SensorOutput done (I think...)
got everything compiling except for missing SensorReceiver
worked on implementing the logic. didn't finish
made everything compile and finished implementing SensorReceiver
pulling over Austin's mock time stuff
added IncrementMockTime
finished up and started on tests
remembered something else
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index d727036..25b358c 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -7,7 +7,7 @@
'<(AOS)/build/aos_all.gyp:Atom',
'../control_loops/control_loops.gyp:DriveTrain',
'../input/input.gyp:JoystickReader',
- '../input/input.gyp:SensorReader',
+ '../input/input.gyp:sensor_receiver',
'../input/input.gyp:GyroReader',
'../input/input.gyp:AutoMode',
'../output/output.gyp:MotorWriter',
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 0edca32..574f73d 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -1,6 +1,6 @@
MotorWriter
JoystickReader
-SensorReader
+sensor_receiver
GyroReader
DriveTrain
AutoMode
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index bd2f078..f122ee3 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -5,6 +5,7 @@
#include "aos/common/messages/RobotState.q.h"
#include "aos/atom_code/output/MotorOutput.h"
+#include "aos/common/logging/logging.h"
namespace frc971 {
namespace constants {
diff --git a/frc971/crio/crio.gyp b/frc971/crio/crio.gyp
index 330a6fa..cbd7d2d 100644
--- a/frc971/crio/crio.gyp
+++ b/frc971/crio/crio.gyp
@@ -22,15 +22,15 @@
'main.cc',
],
'dependencies': [
- '../input/input.gyp:SensorReader',
- '../input/input.gyp:SensorWriter',
'../output/output.gyp:MotorWriter',
- '../output/output.gyp:SensorSender',
'WPILib_changes',
'<(EXTERNALS):WPILib',
'<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/crio/controls/controls.gyp:ControlsManager',
- '<(AOS)/crio/motor_server/motor_server.gyp:CRIOControlLoopRunner',
+ '<(AOS)/crio/motor_server/motor_server.gyp:crio_control_loop_runner',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_broadcaster',
+ '<(DEPTH)/frc971/input/input.gyp:sensor_packer',
+ '<(DEPTH)/frc971/input/input.gyp:sensor_unpacker',
],
},
{
diff --git a/frc971/crio/main.cc b/frc971/crio/main.cc
index e814b97..649946b 100644
--- a/frc971/crio/main.cc
+++ b/frc971/crio/main.cc
@@ -1,17 +1,32 @@
#include "aos/crio/controls/ControlsManager.h"
+#include "aos/crio/motor_server/crio_control_loop_runner.h"
+#include "aos/common/sensors/sensor_broadcaster.h"
-#include "aos/crio/motor_server/CRIOControlLoopRunner.h"
+#include "frc971/queues/sensor_values.h"
+#include "frc971/input/sensor_packer.h"
+#include "frc971/input/sensor_unpacker.h"
namespace frc971 {
class MyRobot : public ::aos::crio::ControlsManager {
public:
+ MyRobot()
+ : broadcaster_(&packer_),
+ control_loop_runner_(&broadcaster_, &unpacker_) {}
+
virtual void RegisterControlLoops() {
- //::aos::crio::CRIOControlLoopRunner::AddControlLoop(&shooter_);
+ //control_loop_runner_.AddControlLoop(&shooter_);
}
- private:
- //::frc971::control_loops::ShooterMotor shooter_;
+ virtual void StartSensorBroadcasters() {
+ broadcaster_.Start();
+ }
+
+ ::frc971::SensorPacker packer_;
+ ::frc971::SensorUnpacker unpacker_;
+ ::aos::sensors::SensorBroadcaster< ::frc971::sensor_values> broadcaster_;
+ ::aos::crio::CRIOControlLoopRunner< ::frc971::sensor_values>
+ control_loop_runner_;
};
} // namespace frc971
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index ff43949..89008cf 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -7,7 +7,9 @@
'constants.cpp',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/output/output.gyp:motor_output',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
],
}
],
diff --git a/frc971/input/GyroReader.cc b/frc971/input/GyroReader.cc
index 9fbee44..bdcd07e 100644
--- a/frc971/input/GyroReader.cc
+++ b/frc971/input/GyroReader.cc
@@ -4,6 +4,7 @@
#include <fcntl.h>
#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
#include "frc971/queues/GyroAngle.q.h"
diff --git a/frc971/input/SensorReader.cc b/frc971/input/SensorReader.cc
deleted file mode 100644
index aed0818..0000000
--- a/frc971/input/SensorReader.cc
+++ /dev/null
@@ -1,43 +0,0 @@
-#define __STDC_LIMIT_MACROS
-
-#include <arpa/inet.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
deleted file mode 100644
index 1fb34db..0000000
--- a/frc971/input/SensorWriter.cc
+++ /dev/null
@@ -1,40 +0,0 @@
-#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
index 99bbb3e..7ff9e3f 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -27,35 +27,44 @@
],
},
{
- 'target_name': 'SensorReader',
- 'type': '<(aos_target)',
+ 'target_name': 'sensor_unpacker',
+ 'type': 'static_library',
'sources': [
- 'SensorReader.cc',
+ 'sensor_unpacker.cc',
],
'dependencies': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/input/input.gyp:sensor_input',
- '<(AOS)/build/aos.gyp:aos_core',
- ],
- 'conditions': [
- ['OS!="crio"', {
- 'dependencies': [
- '<(AOS)/atom_code/atom_code.gyp:init',
- ],
- }],
],
},
{
- 'target_name': 'SensorWriter',
- 'type': '<(aos_target)',
+ 'target_name': 'sensor_receiver',
+ 'type': 'executable',
'sources': [
- 'SensorWriter.cc',
+ 'sensor_receiver.cc',
],
'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'sensor_unpacker',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ ],
+ },
+ {
+ 'target_name': 'sensor_packer',
+ 'type': 'static_library',
+ 'sources': [
+ 'sensor_packer.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/common.gyp:mutex',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
+ '<(AOS)/common/common.gyp:mutex',
],
},
{
diff --git a/frc971/input/sensor_packer.cc b/frc971/input/sensor_packer.cc
new file mode 100644
index 0000000..b164e45
--- /dev/null
+++ b/frc971/input/sensor_packer.cc
@@ -0,0 +1,23 @@
+#include "frc971/input/sensor_packer.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+
+using ::aos::MutexLocker;
+
+namespace frc971 {
+
+SensorPacker::SensorPacker() : lencoder(1, 2), rencoder(3, 4) {
+ lencoder.Start();
+ rencoder.Start();
+
+ printf("frc971::SensorPacker started\n");
+}
+
+void SensorPacker::PackInto(sensor_values *values) {
+ values->lencoder = htonl(-lencoder.GetRaw());
+ values->rencoder = -htonl(-rencoder.GetRaw());
+}
+
+} // namespace frc971
diff --git a/frc971/input/sensor_packer.h b/frc971/input/sensor_packer.h
new file mode 100644
index 0000000..855431f
--- /dev/null
+++ b/frc971/input/sensor_packer.h
@@ -0,0 +1,30 @@
+#ifndef FRC971_INPUT_SENSOR_PACKER_H_
+#define FRC971_INPUT_SENSOR_PACKER_H_
+
+#include "aos/common/mutex.h"
+#include "aos/crio/shared_libs/interrupt_notifier.h"
+#include "aos/common/sensors/sensor_packer.h"
+#include "WPILib/Task.h"
+#include "WPILib/Encoder.h"
+#include "WPILib/DigitalInput.h"
+#include "WPILib/Counter.h"
+
+#include "frc971/queues/sensor_values.h"
+
+namespace frc971 {
+
+class SensorPacker
+ : public ::aos::sensors::SensorPackerInterface<sensor_values> {
+ public:
+ SensorPacker();
+
+ virtual void PackInto(sensor_values *values);
+
+ private:
+ Encoder lencoder;
+ Encoder rencoder;
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_SENSOR_PACKER_H_
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
new file mode 100644
index 0000000..a0b7f8f
--- /dev/null
+++ b/frc971/input/sensor_receiver.cc
@@ -0,0 +1,16 @@
+#include "aos/common/sensors/sensor_receiver.h"
+#include "aos/atom_code/init.h"
+
+#include "frc971/queues/sensor_values.h"
+#include "frc971/input/sensor_unpacker.h"
+
+int main() {
+ ::aos::Init();
+ ::frc971::SensorUnpacker unpacker;
+ ::aos::sensors::NetworkSensorReceiver< ::frc971::sensor_values>
+ receiver(&unpacker);
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
new file mode 100644
index 0000000..95e30da
--- /dev/null
+++ b/frc971/input/sensor_unpacker.cc
@@ -0,0 +1,40 @@
+#include "frc971/input/sensor_unpacker.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+
+#include "frc971/control_loops/DriveTrain.q.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
+
+SensorUnpacker::SensorUnpacker() {}
+
+void SensorUnpacker::UnpackFrom(sensor_values *values) {
+ for (size_t i = 0; i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
+ values->encoders[i] = ntohl(values->encoders[i]);
+ }
+
+ // TODO(aschuh): Convert to meters.
+ const double left_encoder = drivetrain_translate(values->lencoder);
+ const double right_encoder = drivetrain_translate(values->rencoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+}
+
+} // namespace frc971
diff --git a/frc971/input/sensor_unpacker.h b/frc971/input/sensor_unpacker.h
new file mode 100644
index 0000000..b15244a
--- /dev/null
+++ b/frc971/input/sensor_unpacker.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_INPUT_SENSOR_UNPACKER_H_
+#define FRC971_INPUT_SENSOR_UNPACKER_H_
+
+#include "aos/common/sensors/sensor_unpacker.h"
+
+#include "frc971/queues/sensor_values.h"
+
+namespace frc971 {
+
+class SensorUnpacker
+ : public ::aos::sensors::SensorUnpackerInterface<sensor_values> {
+ public:
+ SensorUnpacker();
+
+ virtual void UnpackFrom(sensor_values *values);
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_SENSOR_UNPACKER_H_
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/AtomMotorWriter.cc
index 243ac81..37d7591 100644
--- a/frc971/output/AtomMotorWriter.cc
+++ b/frc971/output/AtomMotorWriter.cc
@@ -3,7 +3,6 @@
#include <unistd.h>
#include "aos/aos_core.h"
-#include "aos/common/network/SendSocket.h"
#include "aos/common/control_loop/Timing.h"
#include "aos/common/messages/RobotState.q.h"
#include "aos/atom_code/output/MotorOutput.h"
diff --git a/frc971/output/SensorSender.cc b/frc971/output/SensorSender.cc
deleted file mode 100644
index 105088d..0000000
--- a/frc971/output/SensorSender.cc
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "aos/crio/motor_server/SensorSender.h"
-#include "aos/aos_core.h"
-
-#include "frc971/queues/sensor_values.h"
-
-AOS_RUN_FORK(aos::SensorSender<frc971::sensor_values>, "971SS", 100)
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 42bfe94..caa4bb8 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -32,7 +32,6 @@
'<(AOS)/atom_code/output/output.gyp:motor_output',
'<(AOS)/atom_code/messages/messages.gyp:messages',
'<(AOS)/atom_code/atom_code.gyp:init',
- '<(AOS)/common/network/network.gyp:socket',
],
}, {
'sources': ['CRIOMotorWriter.cc'],
@@ -46,15 +45,5 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],
},
- {
- 'target_name': 'SensorSender',
- 'type': '<(aos_target)',
- 'sources': [
- 'SensorSender.cc',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- ],
- },
],
}