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/aos/crio/motor_server/CRIOControlLoopRunner.cpp b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
new file mode 100644
index 0000000..9d6cd52
--- /dev/null
+++ b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
@@ -0,0 +1,49 @@
+#include "CRIOControlLoopRunner.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+#include "aos/crio/motor_server/MotorOutput.h"
+
+using ::aos::control_loops::SerializableControlLoop;
+
+namespace aos {
+namespace crio {
+
+bool CRIOControlLoopRunner::started_ = false;
+std::vector<SerializableControlLoop *> CRIOControlLoopRunner::loops_;
+Mutex CRIOControlLoopRunner::loops_lock;
+
+void CRIOControlLoopRunner::Start() {
+ if (started_) {
+ LOG(WARNING, "not going to Start twice!!\n");
+ return;
+ }
+ started_ = true;
+
+ // TODO(aschuh): Hold on to a handle to this...
+ (new WDInterruptNotifier<void>(Notify))->StartPeriodic(0.01);
+}
+
+void CRIOControlLoopRunner::AddControlLoop(SerializableControlLoop *loop) {
+ MutexLocker control_loop_goals_locker(&loops_lock);
+ loops_.push_back(loop);
+ MotorServer::RegisterControlLoopGoal(loop);
+}
+
+void CRIOControlLoopRunner::Notify(void *) {
+ // TODO(aschuh): Too many singletons/static classes!
+ SensorOutputs::UpdateAll();
+ // sensors get read first so it doesn't really matter if this takes a little bit
+ {
+ MutexLocker control_loop_goals_locker(
+ &MotorServer::control_loop_goals_lock);
+ for (auto it = loops_.begin(); it != loops_.end(); ++it) {
+ (*it)->Iterate();
+ }
+ }
+ MotorOutput::RunIterationAll();
+ MotorServer::WriteOutputs();
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.h b/aos/crio/motor_server/CRIOControlLoopRunner.h
new file mode 100644
index 0000000..efed120
--- /dev/null
+++ b/aos/crio/motor_server/CRIOControlLoopRunner.h
@@ -0,0 +1,40 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
+#define AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
+
+#include <vector>
+#include <semLib.h>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/mutex.h"
+
+namespace aos {
+namespace crio {
+
+// Runs crio-side control loops. Completely static because there is no reason
+// for multiple ones and it gets rid of the problem of passing an instance
+// around.
+class CRIOControlLoopRunner {
+ public:
+ // Spawns a new Task that loops forever.
+ // No other functions should be called before this one returns.
+ static void Start();
+
+ // Adds a control loop to run.
+ // This class takes control of the instance.
+ static void AddControlLoop(control_loops::SerializableControlLoop *loop);
+
+ private:
+ static bool started_;
+
+ static std::vector<control_loops::SerializableControlLoop *> loops_;
+ static Mutex loops_lock;
+
+ // Gets called by a WDInterruptNotifier on 0.01 second intervals.
+ static void Notify(void *);
+};
+
+
+} // namespace crio
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/ControlLoopGoals.h b/aos/crio/motor_server/ControlLoopGoals.h
new file mode 100644
index 0000000..f22c0a2
--- /dev/null
+++ b/aos/crio/motor_server/ControlLoopGoals.h
@@ -0,0 +1,44 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_CONTROL_LOOP_GOAL_H_
+#define AOS_CRIO_MOTOR_SERVER_CONTROL_LOOP_GOAL_H_
+
+#include <vector>
+#include <semLib.h>
+
+namespace aos {
+
+// This class is used to keep track of all the control loop goals. It exists
+// because of several bugs discovered in the std::map implementation.
+class ControlLoopGoals {
+ public:
+ struct Goal {
+ const char *const name;
+ const size_t length;
+ void (*const zero)();
+ void (*const ntoh)(const char *);
+ Goal(const char *name, size_t length, void (*zero)(), void (*ntoh)(const char *)) :
+ name(name), length(length), zero(zero), ntoh(ntoh) {}
+ };
+
+ private:
+ std::vector<Goal *> goals_;
+
+ public:
+ ControlLoopGoals() {}
+ void Add(const char *name, size_t length, void (*zero)(), void (*ntoh)(const char *)) {
+ char *storage = new char[10];
+ memcpy(storage, name, sizeof(storage));
+ goals_.push_back(new Goal(storage, length, zero, ntoh));
+ }
+ const Goal *Get(const char *name) {
+ for (auto it = goals_.begin(); it != goals_.end(); ++it) {
+ if (memcmp((*it)->name, name, sizeof((*it)->name)) == 0) {
+ return *it;
+ }
+ }
+ return NULL;
+ }
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/MotorControllerOutput.cpp b/aos/crio/motor_server/MotorControllerOutput.cpp
new file mode 100644
index 0000000..a0897b4
--- /dev/null
+++ b/aos/crio/motor_server/MotorControllerOutput.cpp
@@ -0,0 +1,103 @@
+#include "aos/crio/motor_server/MotorControllerOutput.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/byteorder.h"
+#include "aos/common/commonmath.h"
+
+namespace aos {
+
+void LinearizedVictor::Set(float speed, UINT8 syncGroup) {
+ speed_ = speed;
+ Victor::Set(Linearize(speed), syncGroup);
+}
+
+float LinearizedVictor::Get() {
+ return speed_;
+}
+
+void LinearizedVictor::Disable() {
+ Victor::Disable();
+ speed_ = 0.0;
+}
+
+double LinearizedVictor::Linearize(double goal_speed) {
+ // These values were derived by putting the robot up on blocks, and driving it
+ // at various speeds. The power required to drive at these speeds was then
+ // recorded and fit with gnuplot.
+ const double deadband_value = 0.082;
+ // If we are outside the range such that the motor is actually moving,
+ // subtract off the constant offset from the deadband. This makes the
+ // function odd and intersect the origin, making the fitting easier to do.
+ if (goal_speed > deadband_value) {
+ goal_speed -= deadband_value;
+ } else if (goal_speed < -deadband_value) {
+ goal_speed += deadband_value;
+ } else {
+ goal_speed = 0.0;
+ }
+ goal_speed = goal_speed / (1.0 - deadband_value);
+
+ double goal_speed2 = goal_speed * goal_speed;
+ double goal_speed3 = goal_speed2 * goal_speed;
+ double goal_speed4 = goal_speed3 * goal_speed;
+ double goal_speed5 = goal_speed4 * goal_speed;
+ double goal_speed6 = goal_speed5 * goal_speed;
+ double goal_speed7 = goal_speed6 * goal_speed;
+
+ // Constants for the 5th order polynomial
+ double victor_fit_e1 = 0.437239;
+ double victor_fit_c1 = -1.56847;
+ double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0
+ * victor_fit_c1 - 116.0) / 125.0);
+ double answer_5th_order = (victor_fit_a1 * goal_speed5
+ + victor_fit_c1 * goal_speed3
+ + victor_fit_e1 * goal_speed);
+
+ // Constants for the 7th order polynomial
+ double victor_fit_c2 = -5.46889;
+ double victor_fit_e2 = 2.24214;
+ double victor_fit_g2 = -0.042375;
+ double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2
+ + victor_fit_g2) - 116.0) / 125.0);
+ double answer_7th_order = (victor_fit_a2 * goal_speed7
+ + victor_fit_c2 * goal_speed5
+ + victor_fit_e2 * goal_speed3
+ + victor_fit_g2 * goal_speed);
+
+
+ // Average the 5th and 7th order polynomials, and add a bit of linear power in
+ // as well. The average turns out to nicely fit the data in gnuplot with nice
+ // smooth curves, and the linear power gives it a bit more punch at low speeds
+ // again. Stupid victors.
+ double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ + .15 * goal_speed * (1.0 - deadband_value);
+
+ // Add the deadband power back in to make it so that the motor starts moving
+ // when any power is applied. This is what the fitting assumes.
+ if (answer > 0.001) {
+ answer += deadband_value;
+ } else if (answer < -0.001) {
+ answer -= deadband_value;
+ }
+
+ return Clip(answer, -1.0, 1.0);
+}
+
+bool MotorControllerOutput::ReadValue(ByteBuffer &buff) {
+ const float val = buff.read_float();
+ if (val == (1.0 / 0.0)) {
+ return false;
+ }
+ value = val;
+ return true;
+}
+void MotorControllerOutput::SetValue() {
+ output.Set(value);
+}
+void MotorControllerOutput::NoValue() {
+ // this is NOT a Set(0.0); it's the same as when the robot is disabled
+ output.Disable();
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/MotorControllerOutput.h b/aos/crio/motor_server/MotorControllerOutput.h
new file mode 100644
index 0000000..41c3546
--- /dev/null
+++ b/aos/crio/motor_server/MotorControllerOutput.h
@@ -0,0 +1,68 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_MOTOR_CONTROLLER_OUTPUT_H_
+#define AOS_CRIO_MOTOR_SERVER_MOTOR_CONTROLLER_OUTPUT_H_
+
+#include "aos/crio/motor_server/OutputDevice.h"
+
+#include "aos/crio/Talon.h"
+
+#include "WPILib/SpeedController.h"
+#include "WPILib/Jaguar.h"
+#include "WPILib/CANJaguar.h"
+#include "WPILib/Victor.h"
+
+namespace aos {
+
+// LinearizedVictor is a Victor that transforms the set values to linearize the
+// hardware's response curve.
+class LinearizedVictor : public Victor {
+ public:
+ explicit LinearizedVictor(uint32_t channel) : Victor(channel), speed_(0) {}
+ virtual void Set(float speed, UINT8 syncGroup=0);
+ virtual float Get();
+ virtual void Disable();
+
+ // Returns the linearized motor power to apply to get the motor to go at the
+ // provided goal_speed.
+ static double Linearize(double goal_speed);
+
+ private:
+ // The speed last sent to the Victor.
+ float speed_;
+};
+
+class MotorControllerOutput : public OutputDevice {
+ private:
+ SpeedController &output;
+ protected:
+ double value;
+ MotorControllerOutput(SpeedController *output) : OutputDevice(), output(*output) {
+ value = 0.0;
+ }
+ // TODO(brians) add virtual destructor?
+
+ virtual bool ReadValue(ByteBuffer &buff);
+ virtual void SetValue();
+ virtual void NoValue();
+};
+
+class JaguarOutput : public MotorControllerOutput {
+ public:
+ JaguarOutput(uint32_t port) : MotorControllerOutput(new Jaguar(port)) {}
+};
+class CANJaguarOutput : public MotorControllerOutput {
+ public:
+ CANJaguarOutput(uint32_t port) : MotorControllerOutput(new CANJaguar(port)) {}
+};
+class VictorOutput : public MotorControllerOutput {
+ public:
+ VictorOutput(uint32_t port)
+ : MotorControllerOutput(new LinearizedVictor(port)) {}
+};
+class TalonOutput : public MotorControllerOutput {
+ public:
+ TalonOutput(uint32_t port) : MotorControllerOutput(new Talon(port)) {}
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/MotorOutput.cpp b/aos/crio/motor_server/MotorOutput.cpp
new file mode 100644
index 0000000..f2e9925
--- /dev/null
+++ b/aos/crio/motor_server/MotorOutput.cpp
@@ -0,0 +1,22 @@
+#include "MotorOutput.h"
+
+namespace aos {
+
+SEM_ID MotorOutput::lock = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+std::vector<MotorOutput *> MotorOutput::instances;
+
+void MotorOutput::Run() {
+ semTake(lock, WAIT_FOREVER);
+ instances.push_back(this);
+ semGive(lock);
+}
+void MotorOutput::RunIterationAll() {
+ semTake(lock, WAIT_FOREVER);
+ for (auto it = instances.begin(); it != instances.end(); ++it) {
+ (*it)->RunIteration();
+ }
+ semGive(lock);
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/MotorOutput.h b/aos/crio/motor_server/MotorOutput.h
new file mode 100644
index 0000000..af7c803
--- /dev/null
+++ b/aos/crio/motor_server/MotorOutput.h
@@ -0,0 +1,27 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_MOTOR_OUTPUT_H_
+#define AOS_CRIO_MOTOR_SERVER_MOTOR_OUTPUT_H_
+
+#include <vector>
+#include <semLib.h>
+
+namespace aos {
+
+// The place where the outputs from crio control loops get written out to the
+// motors.
+class MotorOutput {
+ public:
+ // Call RunIteration on all instances that have been Run.
+ static void RunIterationAll();
+ void Run();
+ protected:
+ // Write the outputs from crio control loops to wherever they go.
+ virtual void RunIteration() = 0;
+ private:
+ static std::vector<MotorOutput *> instances;
+ static SEM_ID lock;
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/motor_server/MotorServer.cpp b/aos/crio/motor_server/MotorServer.cpp
new file mode 100644
index 0000000..77a0579
--- /dev/null
+++ b/aos/crio/motor_server/MotorServer.cpp
@@ -0,0 +1,227 @@
+#include "aos/crio/motor_server/MotorServer.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "usrLib.h"
+#include "aos/common/inttypes.h"
+
+#include "WPILib/Timer.h"
+#include "WPILib/Task.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/motor_server/MotorControllerOutput.h"
+#include "aos/crio/motor_server/SolenoidOutput.h"
+#include "aos/common/Configuration.h"
+
+namespace aos {
+namespace crio {
+
+ByteBuffer MotorServer::buff(4096);
+DriverStationLCD *MotorServer::ds_lcd(NULL);
+int MotorServer::count(0);
+ReceiveSocket *MotorServer::sock(NULL);
+SEM_ID MotorServer::motorSync = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+OutputDevice *MotorServer::output_devices[256][kMaxOutputDeviceNumber];
+Mutex MotorServer::control_loop_goals_lock;
+::std::map<uint32_t,
+ control_loops::SerializableControlLoop *> MotorServer::loops;
+Task *MotorServer::tcpTask;
+void MotorServer::Start() {
+ sock = new ReceiveSocket(NetworkPort::kMotors);
+
+ memset(output_devices, 0x00, sizeof(output_devices));
+
+ tcpTask = new Task("MRLoop",
+ reinterpret_cast<FUNCPTR>(RunReaderTask),
+ WORK_PRIORITY);
+
+ tcpTask->Start();
+}
+
+void MotorServer::ProcessBuf() {
+ semTake(motorSync, WAIT_FOREVER);
+ bool cont = true;
+ while (true) {
+ if (!cont) {
+ LOG(WARNING, "Malformed Packet\n");
+ goto end;
+ }
+ switch (const int value = buff.read_char()) {
+ case 'g':
+ cont = ProcessControlLoopGoal();
+ break;
+ case 'd':
+ cont = ProcessDSLine();
+ break;
+ case -1:
+ goto end;
+ default:
+ cont = ProcessOutputDevice(value);
+ break;
+ }
+ }
+end:
+ ++count;
+ semGive(motorSync);
+}
+bool MotorServer::ProcessOutputDevice(const int type) {
+ const int id = buff.read_char(); // 1-indexed
+ if (id < 1 || id > static_cast<ssize_t>(kMaxOutputDeviceNumber)) {
+ if (id != -1) {
+ LOG(ERROR, "illegal OutputDevice id %d\n", id);
+ }
+ return false;
+ }
+
+ if (output_devices[type][id - 1] == NULL) {
+ switch (type) {
+ case 'v':
+ output_devices[type][id - 1] = new VictorOutput(id);
+ break;
+ case 'j':
+ output_devices[type][id - 1] = new JaguarOutput(id);
+ break;
+ case 'c':
+ output_devices[type][id - 1] = new CANJaguarOutput(id);
+ break;
+ case 't':
+ output_devices[type][id - 1] = new TalonOutput(id);
+ break;
+ case 's':
+ output_devices[type][id - 1] = new SolenoidOutput(id);
+ break;
+ default:
+ LOG(ERROR, "unrecognized OutputDevice type %d\n", type);
+ return false;
+ }
+ }
+ return output_devices[type][id - 1]->ReadValue(buff);
+}
+
+bool MotorServer::ProcessDSLine() {
+ int line = buff.read_char();
+ if (line == -1) {
+ return false;
+ }
+ // TODO(brians): Subfunction
+ DriverStationLCD::Line ds_line;
+ switch (line) {
+ case 0:
+ ds_line = DriverStationLCD::kMain_Line6;
+ break;
+ case 1:
+ ds_line = DriverStationLCD::kUser_Line1;
+ break;
+ case 2:
+ ds_line = DriverStationLCD::kUser_Line2;
+ break;
+ case 3:
+ ds_line = DriverStationLCD::kUser_Line3;
+ break;
+ case 4:
+ ds_line = DriverStationLCD::kUser_Line4;
+ break;
+ case 5:
+ ds_line = DriverStationLCD::kUser_Line5;
+ break;
+ case 6:
+ ds_line = DriverStationLCD::kUser_Line6;
+ break;
+ default:
+ LOG(ERROR, "illegal line number %hhd\n", line);
+ return false;
+ }
+ // TODO(brians) see if this mess with not creating the DriverStationLCD for a
+ // bit is actually needed
+ static int ds_lcd_counts = 0; // to prevent crashes at startup
+ if (ds_lcd == NULL) {
+ if (ds_lcd_counts < 100) {
+ ++ds_lcd_counts;
+ } else {
+ ++ds_lcd_counts;
+ ds_lcd = DriverStationLCD::GetInstance();
+ }
+ }
+ char buf[DriverStationLCD::kLineLength];
+ buff.read_string(buf, sizeof(buf));
+ buf[sizeof(buf) - 1] = 0;
+ if (ds_lcd != NULL) {
+ ds_lcd->PrintfLine(ds_line, "%s", buf);
+ }
+ return true;
+}
+
+void MotorServer::RegisterControlLoopGoal(
+ control_loops::SerializableControlLoop *control_loop) {
+ uint32_t unique_id = control_loop->UniqueID();
+
+ bool replaced;
+ {
+ MutexLocker control_loop_goals_locker(&control_loop_goals_lock);
+ replaced = !InsertIntoMap(&loops, unique_id, control_loop);
+ }
+ if (replaced) {
+ LOG(ERROR, "Replaced a key for unique id 0x%"PRIx32"\n", unique_id);
+ }
+}
+
+bool MotorServer::ProcessControlLoopGoal() {
+ // Read back a uint32_t with the hash.
+ uint32_t hash;
+ if (!buff.read_uint32(&hash)) return false;
+ MutexLocker control_loop_goals_locker(&control_loop_goals_lock);
+
+ control_loops::SerializableControlLoop *loop;
+ if (!GetFromMap(loops, hash, &loop)) {
+ return false;
+ }
+ const size_t length = loop->SeralizedSize();
+ char *const goal_bytes = buff.get_bytes(length);
+ if (goal_bytes == NULL) {
+ return false;
+ } else {
+ loop->Deserialize(goal_bytes);
+ }
+ return true;
+}
+
+void MotorServer::RunReaderTask() {
+ while (true) {
+ if (buff.recv_from_sock(sock)) {
+ ProcessBuf();
+ }
+ }
+}
+void MotorServer::WriteOutputs() {
+ static int last_count = 0, bad_counts = 0;
+ semTake(motorSync, WAIT_FOREVER);
+ if (last_count != count) {
+ bad_counts = 0;
+ } else {
+ ++bad_counts;
+ }
+ last_count = count;
+ // both loops iterate over all elements of output_devices by indexing off the
+ // end of output_devices[0]
+ if (bad_counts > 2) {
+ LOG(WARNING, "no new values. stopping all outputs\n");
+ for (size_t i = 0; i < sizeof(output_devices) / sizeof(output_devices[0][0]); ++i) {
+ if (output_devices[0][i] != NULL) {
+ output_devices[0][i]->NoValue();
+ }
+ }
+ } else {
+ for (size_t i = 0; i < sizeof(output_devices) / sizeof(output_devices[0][0]); ++i) {
+ if (output_devices[0][i] != NULL) {
+ output_devices[0][i]->SetValue();
+ }
+ }
+ }
+ if (ds_lcd != NULL) {
+ ds_lcd->UpdateLCD();
+ }
+ semGive(motorSync);
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/motor_server/MotorServer.h b/aos/crio/motor_server/MotorServer.h
new file mode 100644
index 0000000..0126663
--- /dev/null
+++ b/aos/crio/motor_server/MotorServer.h
@@ -0,0 +1,87 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_MOTOR_SERVER_H_
+#define AOS_CRIO_MOTOR_SERVER_MOTOR_SERVER_H_
+
+#include <vxWorks.h>
+#include <timers.h>
+#include <string.h>
+#include "WPILib/Task.h"
+#include "WPILib/Victor.h"
+#include "WPILib/Jaguar.h"
+#include "WPILib/Solenoid.h"
+#include "sockLib.h"
+#include <inetLib.h>
+#include <stdio.h>
+#include <selectLib.h>
+#include <stdlib.h>
+#include <time.h>
+#include <map>
+#include <string>
+
+#include "WPILib/DriverStationLCD.h"
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/messages/QueueHolder.h"
+#include "aos/common/mutex.h"
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/crio/motor_server/ControlLoopGoals.h"
+#include "aos/crio/motor_server/OutputDevice.h"
+#include "aos/crio/motor_server/SensorSender.h"
+#include "aos/crio/shared_libs/ByteBuffer.h"
+#include "aos/map_utils.h"
+
+namespace aos {
+namespace crio {
+
+class CRIOControlLoopRunner;
+class MotorServer {
+ public:
+ static void Start();
+
+ // Adds the given control loop's goal queue to the list of ones to process.
+ static void RegisterControlLoopGoal(
+ control_loops::SerializableControlLoop *control_loop);
+
+ static const int32_t WORK_PRIORITY = 100;
+
+ private:
+ friend class CRIOControlLoopRunner;
+ // Counter for how many times new values come in. Used to stop all the
+ // outputs if values stop.
+ // Would take days to overflow.
+ static int count;
+ static SEM_ID motorSync;
+ // Gets called by CRIOControlLoopRunner every 10ms after it runs all of the
+ // control loops.
+ static void WriteOutputs();
+
+ static void RunReaderTask();
+ static Task *tcpTask;
+ static ReceiveSocket *sock;
+ static ByteBuffer buff;
+
+ static DriverStationLCD *ds_lcd;
+ static bool ProcessDSLine();
+
+ static const size_t kMaxOutputDeviceNumber = 10;
+ static OutputDevice *output_devices[256][kMaxOutputDeviceNumber];
+ static bool ProcessOutputDevice(const int type);
+
+ // Go through the whole buffer and call the appropriate Process* methods to
+ // process each part.
+ static void ProcessBuf();
+
+ static bool ProcessControlLoopGoal();
+ // Locked whenever adding/using the control loop goals maps.
+ // Also used by CRIOControlLoopRunner while modifying any of the data
+ // structures. Used by both of them while reading/writing from
+ // the goal queues.
+ static Mutex control_loop_goals_lock;
+ static ::std::map<uint32_t, control_loops::SerializableControlLoop *> loops;
+};
+
+} // namespace crio
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/OutputDevice.h b/aos/crio/motor_server/OutputDevice.h
new file mode 100644
index 0000000..0789a68
--- /dev/null
+++ b/aos/crio/motor_server/OutputDevice.h
@@ -0,0 +1,26 @@
+#ifndef __CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
+#define __CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
+
+#include <stdint.h>
+#include "aos/crio/shared_libs/ByteBuffer.h"
+
+namespace aos {
+
+class OutputDevice {
+ protected:
+ OutputDevice() {
+ }
+ public:
+ // Reads the value out of buff and stores it somewhere for SetValue to use.
+ // Returns whether or not it successfully read a whole value out of buff.
+ virtual bool ReadValue(ByteBuffer &buff) = 0;
+ // Actually sets the output device to the value saved by ReadValue.
+ virtual void SetValue() = 0;
+ // Gets called when no values come in for a while.
+ virtual void NoValue() = 0;
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/motor_server/SensorOutput-tmpl.h b/aos/crio/motor_server/SensorOutput-tmpl.h
new file mode 100644
index 0000000..d6b8b69
--- /dev/null
+++ b/aos/crio/motor_server/SensorOutput-tmpl.h
@@ -0,0 +1,27 @@
+#include "aos/common/input/SensorInput.h"
+
+namespace aos {
+
+template<class Values> std::vector<SensorOutput<Values> *> SensorOutput<Values>::output_running_;
+template<class Values> void SensorOutput<Values>::Run() {
+ semTake(lock_, WAIT_FOREVER);
+ output_running_.push_back(this);
+ outputs_running_.push_back(this);
+ semGive(lock_);
+}
+
+template<class Values> void SensorOutput<Values>::RunIterationAll(Values &vals) {
+ semTake(lock_, WAIT_FOREVER);
+ for (auto it = output_running_.begin(); it != output_running_.end(); ++it) {
+ (*it)->RunIteration(vals);
+ }
+ semGive(lock_);
+}
+template<class Values> void SensorOutput<Values>::Update() {
+ Values vals;
+ RunIteration(vals);
+ SensorInput<Values>::RunIterationAll(vals);
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/SensorOutput.cpp b/aos/crio/motor_server/SensorOutput.cpp
new file mode 100644
index 0000000..b887885
--- /dev/null
+++ b/aos/crio/motor_server/SensorOutput.cpp
@@ -0,0 +1,17 @@
+#include "aos/crio/motor_server/SensorOutput.h"
+
+namespace aos {
+
+SEM_ID SensorOutputs::lock_ = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+std::vector<SensorOutputs *> SensorOutputs::outputs_running_;
+
+void SensorOutputs::UpdateAll() {
+ semTake(lock_, WAIT_FOREVER);
+ for (auto it = outputs_running_.begin(); it != outputs_running_.end(); ++it) {
+ (*it)->Update();
+ }
+ semGive(lock_);
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/SensorOutput.h b/aos/crio/motor_server/SensorOutput.h
new file mode 100644
index 0000000..9e30cb9
--- /dev/null
+++ b/aos/crio/motor_server/SensorOutput.h
@@ -0,0 +1,48 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_SENSOR_OUTPUT_H_
+#define AOS_CRIO_MOTOR_SERVER_SENSOR_OUTPUT_H_
+
+#include <semLib.h>
+#include <vector>
+
+namespace aos {
+
+// Keeps track of instances of all instantiations.
+class SensorOutputs {
+ public:
+ // Calls RunIteration on all instances and then runs all SensorInput
+ // subclasses for that type.
+ static void UpdateAll();
+ private:
+ static SEM_ID lock_;
+ static std::vector<SensorOutputs *> outputs_running_;
+ protected:
+ // Calls RunIteration with a temporary Values instance and then runs all
+ // SensorInput subclasses with the same Values type.
+ virtual void Update() = 0;
+};
+
+// Class for implementing crio code that reads sensor values and puts them into
+// the sensor struct.
+template<class Values> class SensorOutput : public SensorOutputs {
+ protected:
+ // Fills out vals with the current data.
+ // May not be called at anything close to consistent intervals and may be
+ // called simultaneously with different arguments, so it must be reentrant.
+ virtual void RunIteration(Values &vals) = 0;
+ public:
+ // Sets it up so that RunIteration will get called when appropriate.
+ void Run();
+
+ // Calls RunIteration on all instances with vals.
+ static void RunIterationAll(Values &vals);
+ private:
+ static std::vector<SensorOutput<Values> *> output_running_;
+ virtual void Update();
+};
+
+} // namespace aos
+
+#include "SensorOutput-tmpl.h"
+
+#endif
+
diff --git a/aos/crio/motor_server/SensorSender-tmpl.h b/aos/crio/motor_server/SensorSender-tmpl.h
new file mode 100644
index 0000000..93fe73d
--- /dev/null
+++ b/aos/crio/motor_server/SensorSender-tmpl.h
@@ -0,0 +1,21 @@
+#include "WPILib/Task.h"
+#include "WPILib/Timer.h"
+#include "aos/crio/motor_server/SensorOutput.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/Configuration.h"
+
+namespace aos {
+
+template<class Values> void SensorSender<Values>::Run() {
+ SendSocket sock(NetworkPort::kSensors,
+ configuration::GetIPAddress(configuration::NetworkDevice::kAtom));
+ Values vals;
+ while (true) {
+ Wait(0.0015);
+ SensorOutput<Values>::RunIterationAll(vals);
+ sock.Send(&vals, sizeof(vals));
+ }
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/SensorSender.h b/aos/crio/motor_server/SensorSender.h
new file mode 100644
index 0000000..135e1fa
--- /dev/null
+++ b/aos/crio/motor_server/SensorSender.h
@@ -0,0 +1,23 @@
+#ifndef __CRIO_SENSOR_SENDER_H_
+#define __CRIO_SENSOR_SENDER_H_
+
+namespace aos {
+
+// A class that handles sending all of the sensor values to the atom.
+// Designed for an instantiation (aos::SensorSender<X>) to be AOS_RUN_FORKed,
+// NOT a subclass.
+// Values is the type of the struct that will get sent out over the network.
+// Note: it should the same as the instance of TODO(brians) on the atom and any
+// SensorOutput instances that you want to feed into an instance of this.
+template<class Values> class SensorSender {
+ public:
+ // Loops forever.
+ void Run();
+};
+
+} // namespace aos
+
+#include "SensorSender-tmpl.h"
+
+#endif
+
diff --git a/aos/crio/motor_server/SolenoidOutput.h b/aos/crio/motor_server/SolenoidOutput.h
new file mode 100644
index 0000000..6a6c838
--- /dev/null
+++ b/aos/crio/motor_server/SolenoidOutput.h
@@ -0,0 +1,38 @@
+#ifndef __CRIO_MOTOR_SERVER_SOLENOID_OUTPUT_H_
+#define __CRIO_MOTOR_SERVER_SOLENOID_OUTPUT_H_
+
+#include "aos/crio/motor_server/OutputDevice.h"
+
+namespace aos {
+
+class SolenoidOutput : public OutputDevice {
+ private:
+ Solenoid solenoid;
+ bool value;
+ public:
+ SolenoidOutput(uint32_t port) : OutputDevice(), solenoid(port), value(false) {
+ }
+ protected:
+ virtual bool ReadValue(ByteBuffer &buff) {
+ const int on = buff.read_char();
+ if (on != 0 && on != 1) {
+ if (on != -1) {
+ LOG(ERROR, "illegal solenoid value %d\n", on);
+ }
+ return false;
+ }
+ value = on;
+ return true;
+ }
+ virtual void SetValue() {
+ solenoid.Set(value);
+ }
+ virtual void NoValue() {
+ // leave the solenoid in its previous state
+ }
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/motor_server/victor_drive.rb b/aos/crio/motor_server/victor_drive.rb
new file mode 100644
index 0000000..7de9706
--- /dev/null
+++ b/aos/crio/motor_server/victor_drive.rb
@@ -0,0 +1,30 @@
+require "socket"
+$sock = UDPSocket.new()
+$sock.connect("10.59.71.2",9123)
+def short(val)
+ val += 1.0
+ if(val < 0)
+ val = 0
+ end
+ val = (val * 256 * 128).to_i
+ v1 = val / 256
+ v2 = val % 256
+ if(v1 > 255)
+ v1 = 255
+ v2 = 255
+ end
+ return(v1.chr + v2.chr)
+end
+def jaguar(port,val)
+ $sock.send("j#{port.chr}#{short(val)}",0)
+end
+trap(2) do
+ jaguar(4,0)
+ jaguar(3,0)
+ exit!
+end
+while true
+ jaguar(4,Math.cos(Time.now.to_f))
+ jaguar(3,Math.cos(Time.now.to_f))
+ sleep(0.01)
+end