created a NetworkRobot class that sends messages successfully
diff --git a/aos/atom_code/output/MotorOutput.cpp b/aos/atom_code/output/MotorOutput.cpp
deleted file mode 100644
index ecbdf8b..0000000
--- a/aos/atom_code/output/MotorOutput.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-#include "aos/atom_code/output/MotorOutput.h"
-
-#include <math.h>
-
-#include "aos/common/Configuration.h"
-#include "aos/aos_core.h"
-#include "aos/common/byteorder.h"
-#include "aos/common/control_loop/Timing.h"
-#include "aos/atom_code/messages/DriverStationDisplay.h"
-
-namespace aos {
-
-MotorOutput::MotorOutput() : sock(NetworkPort::kMotors,
- configuration::GetIPAddress(configuration::NetworkDevice::kCRIO)) {}
-
-void MotorOutput::Run() {
- while (true) {
- time::PhasedLoopXMS(5, 1000);
- RunIteration();
-
- // doesn't matter if multiple instances end up running this loop at the same
- // time because the queue handles the race conditions
- while (true) {
- const aos::DriverStationDisplay *line = aos::DriverStationDisplay::GetNext();
- if (line != NULL) {
- AddDSLine(line->line, line->data);
- LOG(DEBUG, "got a line %hhd that said '%s'\n", line->line, line->data);
- aos::DriverStationDisplay::Free(line);
- } else {
- break;
- }
- }
-
- if (sock.SendHoldMsg() == -1) {
- LOG(WARNING, "sending outputs failed\n");
- continue;
- } else {
- LOG(DEBUG, "sent outputs\n");
- }
- }
-}
-
-int MotorOutput::AddMotor(char type, uint8_t num, float value) {
- if (sock.hold_msg_len_ + 4 > sock.MAX_MSG) {
- return -1;
- }
- sock.hold_msg_[sock.hold_msg_len_ ++] = type;
- sock.hold_msg_[sock.hold_msg_len_ ++] = num;
- to_network(&value, &sock.hold_msg_[sock.hold_msg_len_]);
- sock.hold_msg_len_ += 4;
- return 0;
-}
-int MotorOutput::AddSolenoid(uint8_t port, bool on) {
- if (sock.hold_msg_len_ + 3 > sock.MAX_MSG) {
- return -1;
- }
- sock.hold_msg_[sock.hold_msg_len_ ++] = 's';
- sock.hold_msg_[sock.hold_msg_len_ ++] = port;
- sock.hold_msg_[sock.hold_msg_len_ ++] = on ? 1 : 0;
- return 0;
-}
-
-int MotorOutput::AddDSLine(uint8_t line, const char *data) {
- size_t data_len = strlen(data); // doesn't include terminating NULL
- if (sock.hold_msg_len_ + 3 + data_len > sock.MAX_MSG) {
- return -1;
- }
-
- sock.hold_msg_[sock.hold_msg_len_ ++] = 'd';
- sock.hold_msg_[sock.hold_msg_len_ ++] = line;
- sock.hold_msg_[sock.hold_msg_len_ ++] = data_len;
- memcpy(&sock.hold_msg_[sock.hold_msg_len_], data, data_len);
- sock.hold_msg_len_ += data_len;
- return 0;
-}
-
-} // namespace aos
-
diff --git a/aos/atom_code/output/MotorOutput.h b/aos/atom_code/output/MotorOutput.h
deleted file mode 100644
index 925139a..0000000
--- a/aos/atom_code/output/MotorOutput.h
+++ /dev/null
@@ -1,74 +0,0 @@
-#ifndef AOS_ATOM_CODE_OUTPUT_MOTOR_OUTPUT_H_
-#define AOS_ATOM_CODE_OUTPUT_MOTOR_OUTPUT_H_
-
-#include <stdint.h>
-#include <string.h>
-#include <algorithm>
-#include <string>
-
-#include "aos/common/network/SendSocket.h"
-#include "aos/common/byteorder.h"
-#include "aos/common/type_traits.h"
-
-namespace aos {
-
-class MotorOutput {
- public:
- MotorOutput();
- void Run();
-
- // Constants for the first argument of AddMotor.
- static const char VICTOR = 'v';
- static const char TALON = 't';
-
- protected:
- // num is 1-indexed
- int AddMotor(char type, uint8_t num, float value);
- int AddSolenoid(uint8_t num, bool on);
- int AddDSLine(uint8_t line, const char *data);
- // loop_queuegroup is expected to be a control loop queue group.
- // This function will fetch the latest goal and serve it.
- template <class T>
- int AddControlLoopGoal(T *loop_queuegroup);
-
- virtual void RunIteration() = 0;
-
- private:
- SendSocket sock;
-};
-
-template <class T>
-int MotorOutput::AddControlLoopGoal(T *loop_queuegroup) {
- typedef typename std::remove_reference<
- decltype(*(loop_queuegroup->goal.MakeMessage().get()))>::type GoalType;
- // TODO(aschuh): This assumes a static serialized message size.
- const uint16_t size = GoalType::Size();
- if (sock.hold_msg_len_ + 4 + 1 + size > sock.MAX_MSG) {
- return -1;
- }
-
- const bool zero = !loop_queuegroup->goal.FetchLatest();
-
- sock.hold_msg_[sock.hold_msg_len_ ++] = 'g';
- const uint32_t hash = loop_queuegroup->hash();
-
- // Place the loop hash at the front.
- to_network(&hash, &sock.hold_msg_[sock.hold_msg_len_]);
- sock.hold_msg_len_ += 4;
-
- if (zero) {
- GoalType zero_message;
- zero_message.Zero();
- zero_message.Serialize(&sock.hold_msg_[sock.hold_msg_len_]);
- sock.hold_msg_len_ += size;
- return -1;
- } else {
- loop_queuegroup->goal->Serialize(&sock.hold_msg_[sock.hold_msg_len_]);
- sock.hold_msg_len_ += size;
- return 0;
- }
-}
-
-} // namespace aos
-
-#endif
diff --git a/aos/atom_code/output/motor_output.cc b/aos/atom_code/output/motor_output.cc
new file mode 100644
index 0000000..d7022f5
--- /dev/null
+++ b/aos/atom_code/output/motor_output.cc
@@ -0,0 +1,85 @@
+#include "aos/atom_code/output/motor_output.h"
+
+#include <math.h>
+
+#include "aos/common/Configuration.h"
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+
+namespace aos {
+
+// 255 = 2.5ms, 0 = 0.5ms (or something close to that)
+// got 211->210 as first transition that made a speed difference and 53->54 on
+// the other side
+// going 2 to each side to make sure we get the full range
+const MotorOutput::MotorControllerBounds MotorOutput::kTalonBounds
+ {213, 135, 132, 129, 50};
+
+uint8_t MotorOutput::MotorControllerBounds::Map(double value) const {
+ if (value == 0.0) return kCenter;
+ if (value > 0.0) {
+ return static_cast<uint8_t>(kDeadbandMax + (value * (kMax - kDeadbandMax)) +
+ 0.5);
+ } else {
+ return static_cast<uint8_t>(kDeadbandMin + (value * (kDeadbandMin - kMin)) +
+ 0.5);
+ }
+}
+
+MotorOutput::MotorOutput() : socket_(NetworkPort::kMotors,
+ configuration::GetIPAddress(configuration::NetworkDevice::kCRIO)) {}
+
+void MotorOutput::Run() {
+ while (true) {
+ time::PhasedLoopXMS(5, 1000);
+
+ values_.digital_module = -1;
+ // 0 means output disabled.
+ memset(&values_.pwm_outputs, 0x00, sizeof(values_.pwm_outputs));
+ values_.digital_output_enables = 0;
+ values_.digital_output_values = 0;
+ values_.pressure_switch_channel = 0;
+ values_.compressor_channel = 0;
+ values_.solenoid_module = -1;
+ values_.solenoid_values = 0;
+
+ RunIteration();
+
+ values_.digital_output_enables = hton(values_.digital_output_enables);
+ values_.digital_output_values = hton(values_.digital_output_values);
+
+ values_.FillInHashValue();
+ values_.hash_value = hton(values_.hash_value);
+
+ if (socket_.Send(&values_, sizeof(values_)) != sizeof(values_)) {
+ LOG(WARNING, "sending outputs failed\n");
+ } else {
+ LOG(DEBUG, "sent outputs\n");
+ }
+ }
+}
+
+void MotorOutput::SetSolenoid(uint8_t channel, bool set) {
+ if (set) {
+ values_.solenoid_values |= 1 << (channel - 1);
+ }
+}
+
+void MotorOutput::SetPWMOutput(uint8_t channel, double value,
+ const MotorControllerBounds &bounds) {
+ values_.pwm_outputs[channel - 1] = bounds.Map(value);
+}
+
+void MotorOutput::DisablePWMOutput(uint8_t channel) {
+ values_.pwm_outputs[channel - 1] = 0;
+}
+
+void MotorOutput::SetDigitalOutput(uint8_t channel, bool value) {
+ const uint8_t shift_amount = 15 - channel;
+ values_.digital_output_enables |= 1 << shift_amount;
+ if (value) {
+ values_.digital_output_values |= 1 << shift_amount;
+ }
+}
+
+} // namespace aos
diff --git a/aos/atom_code/output/motor_output.h b/aos/atom_code/output/motor_output.h
new file mode 100644
index 0000000..2c1af1d
--- /dev/null
+++ b/aos/atom_code/output/motor_output.h
@@ -0,0 +1,68 @@
+#ifndef AOS_ATOM_CODE_OUTPUT_MOTOR_OUTPUT_H_
+#define AOS_ATOM_CODE_OUTPUT_MOTOR_OUTPUT_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <algorithm>
+#include <string>
+
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/byteorder.h"
+#include "aos/common/type_traits.h"
+#include "aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h"
+
+namespace aos {
+
+// A class for sending output values to a cRIO.
+// values_ gets completely reset each time through, so RunIteration() needs to
+// set everything each time.
+class MotorOutput {
+ public:
+ MotorOutput();
+ void Run();
+
+ // A container for holding the constants that WPILib uses for each type of
+ // motor controller to map floating point values to 1-byte PWM values to
+ // actually use.
+ struct MotorControllerBounds {
+ // What 1.0 maps to.
+ const uint8_t kMax;
+ // The smallest value to map a positive speed to.
+ const uint8_t kDeadbandMax;
+ // What 0.0 maps to.
+ const uint8_t kCenter;
+ // The biggest value to map a negative speed to.
+ const uint8_t kDeadbandMin;
+ // What -1.0 maps to.
+ const uint8_t kMin;
+
+ // Applies the mapping.
+ uint8_t Map(double value) const;
+ };
+
+ protected:
+ // Brian got the values here by trying values with hardware on 11/23/12.
+ static const MotorControllerBounds kTalonBounds;
+
+ // Helper methods for filling out values_.
+ // All channels are the 1-indexed numbers that usually go into WPILib.
+ void SetSolenoid(uint8_t channel, bool set);
+ void SetPWMOutput(uint8_t channel, double value,
+ const MotorControllerBounds &bounds);
+ void DisablePWMOutput(uint8_t channel);
+ void SetDigitalOutput(uint8_t channel, bool value);
+
+ // The struct that's going to get sent over.
+ // Gets reset (everything set so that it won't do anything) each time through.
+ NetworkRobotValues values_;
+
+ private:
+ // Subclasses need to actually fill out values_ here.
+ virtual void RunIteration() = 0;
+
+ SendSocket socket_;
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/atom_code/output/motor_output_test.cc b/aos/atom_code/output/motor_output_test.cc
new file mode 100644
index 0000000..1e8d5fb
--- /dev/null
+++ b/aos/atom_code/output/motor_output_test.cc
@@ -0,0 +1,21 @@
+#include "gtest/gtest.h"
+
+#include "aos/atom_code/output/motor_output.h"
+
+namespace aos {
+namespace testing {
+
+TEST(MotorControllerBoundsTest, Limits) {
+ MotorOutput::MotorControllerBounds test_bounds
+ {200, 135, 128, 126, 110};
+ EXPECT_EQ(test_bounds.Map(1.0), test_bounds.kMax);
+ EXPECT_EQ(test_bounds.Map(-1.0), test_bounds.kMin);
+ EXPECT_EQ(test_bounds.Map(0.0), test_bounds.kCenter);
+ EXPECT_EQ(test_bounds.Map(0.55), 171);
+ EXPECT_EQ(test_bounds.Map(0.5), 168);
+ EXPECT_EQ(test_bounds.Map(0.45), 164);
+ EXPECT_EQ(test_bounds.Map(-0.5), 118);
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/atom_code/output/output.gyp b/aos/atom_code/output/output.gyp
index 65232b6..2dfcd0e 100644
--- a/aos/atom_code/output/output.gyp
+++ b/aos/atom_code/output/output.gyp
@@ -24,14 +24,28 @@
'target_name': 'motor_output',
'type': 'static_library',
'sources': [
- 'MotorOutput.cpp',
+ 'motor_output.cc',
],
'dependencies': [
'<(AOS)/common/network/network.gyp:socket',
- '<(AOS)/build/aos.gyp:aos_core',
+ '<(AOS)/common/common.gyp:common',
+ '<(AOS)/common/common.gyp:timing',
+ '<(EXTERNALS):WPILib-NetworkRobotValues',
],
'export_dependent_settings': [
'<(AOS)/common/network/network.gyp:socket',
+ '<(EXTERNALS):WPILib-NetworkRobotValues',
+ ],
+ },
+ {
+ 'target_name': 'motor_output_test',
+ 'type': 'executable',
+ 'sources': [
+ 'motor_output_test.cc',
+ ],
+ 'dependencies': [
+ 'motor_output',
+ '<(EXTERNALS):gtest',
],
},
],
diff --git a/aos/build/externals.gyp b/aos/build/externals.gyp
index 0daa229..06bd225 100644
--- a/aos/build/externals.gyp
+++ b/aos/build/externals.gyp
@@ -43,6 +43,21 @@
},
},
{
+ 'target_name': 'WPILib-NetworkRobotValues',
+ 'type': 'static_library',
+ 'sources': [
+ '<(externals)/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp'
+ ],
+ 'include_dirs': [
+ '<(externals)/WPILib',
+ ],
+ 'direct_dependent_settings': {
+ 'include_dirs': [
+ '<(externals)/WPILib',
+ ],
+ },
+ },
+ {
'target_name': 'onejar',
'type': 'none',
'direct_dependent_settings': {
diff --git a/aos/common/network/SendSocket.cpp b/aos/common/network/SendSocket.cpp
index 78aca79..c30ec32 100644
--- a/aos/common/network/SendSocket.cpp
+++ b/aos/common/network/SendSocket.cpp
@@ -25,18 +25,7 @@
last_ret_ = 1;
}
- hold_msg_len_ = 0;
return last_ret_ = 0;
}
-int SendSocket::SendHoldMsg() {
- if (hold_msg_len_ > 0) {
- int val = Send(hold_msg_, hold_msg_len_);
- hold_msg_len_ = 0;
- return val;
- }
- return -1;
-}
-
-} // namespace aos
-
+} // namespace aos
diff --git a/aos/common/network/SendSocket.h b/aos/common/network/SendSocket.h
index a0c1fe2..fca904a 100644
--- a/aos/common/network/SendSocket.h
+++ b/aos/common/network/SendSocket.h
@@ -7,7 +7,6 @@
class SendSocket : public Socket {
public:
- //inline int Send(const void *buf, int length) { return Socket::Send(buf, length); }
// Connect must be called before use.
SendSocket() {}
// Calls Connect automatically.
@@ -15,11 +14,6 @@
Connect(port, robot_ip);
}
int Connect(NetworkPort port, const char *robot_ip, int type = SOCK_DGRAM);
-
- static const size_t MAX_MSG = 4096;
- char hold_msg_[MAX_MSG];
- size_t hold_msg_len_;
- int SendHoldMsg();
};
} // namespace aos
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
new file mode 100644
index 0000000..bb665a1
--- /dev/null
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.cpp
@@ -0,0 +1,302 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "WPILib/NetworkRobot/NetworkRobot.h"
+
+#include <sockLib.h>
+#include <stdint.h>
+#include <selectLib.h>
+#include <assert.h>
+
+#include "WPILib/Utility.h"
+#include "WPILib/WPIErrors.h"
+#include "WPILib/SensorBase.h"
+#include "WPILib/DriverStation.h"
+#include "WPILib/Timer.h"
+
+const double NetworkRobot::kDisableTime = 0.15;
+
+NetworkRobot::NetworkRobot(UINT16 port, const char *sender_address)
+ : port_(port), sender_address_(sender_address), socket_(-1),
+ last_received_timestamp_(0.0),
+ digital_modules_(), solenoid_bases_(),
+ allocated_digital_outputs_() {
+}
+NetworkRobot::~NetworkRobot() {
+ if (socket_ != -1) {
+ // Nothing we can do about any errors.
+ close(socket_);
+ }
+ for (size_t i = 0;
+ i < sizeof(solenoid_bases_) / sizeof(solenoid_bases_[0]);
+ ++i) {
+ delete solenoid_bases_[i];
+ }
+ for (size_t module = 0;
+ module < sizeof(digital_modules_) / sizeof(digital_modules_[0]);
+ ++module) {
+ for (int i = 0; i < 16; ++i) {
+ if (allocated_digital_outputs_[module] & (1 << i)) {
+ digital_modules_[module]->FreeDIO(15 - i);
+ }
+ }
+ }
+}
+
+void NetworkRobot::StartCompetition() {
+ // Give ourself plenty of time to get around to feeding it before it
+ // completely cuts out.
+ m_watchdog.SetExpiration(kDisableTime * 2);
+
+ CreateSocket();
+ if (StatusIsFatal()) return;
+
+ errno = 0;
+ if (inet_aton(const_cast<char *>(sender_address_),
+ &expected_sender_address_) == ERROR) {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "Converting IP Address %s", sender_address_);
+ wpi_setErrnoErrorWithContext(buf);
+ return;
+ }
+
+ while (!StatusIsFatal()) {
+ if ((Timer::GetPPCTimestamp() - last_received_timestamp_) > kDisableTime) {
+ StopOutputs();
+ }
+ ReceivePacket();
+ }
+ StopOutputs();
+}
+
+void NetworkRobot::StopOutputs() {
+ for (size_t module = 0;
+ module < sizeof(digital_modules_) / sizeof(digital_modules_[0]);
+ ++module) {
+ DigitalModule *digital_module = digital_modules_[module];
+
+ if (digital_module != NULL) {
+ for (int i = 0; i < 10; ++i) {
+ // 0 means stop sending anything.
+ digital_module->SetPWM(i + 1, 0);
+ }
+
+ // Turn off all of the ones that we're responsible for.
+ digital_module->SetDIOs(allocated_digital_outputs_[module], 0);
+
+ // Turn off all of the relays (both directions).
+ digital_module->SetRelaysForward(0xFF, 0);
+ digital_module->SetRelaysReverse(0xFF, 0);
+ }
+ }
+
+ // Can't do anything intelligent with solenoids. Turning them off can be just
+ // as dangerous as leaving them on.
+
+ // We took care of it, so we don't want the watchdog to permanently disable
+ // us.
+ m_watchdog.Feed();
+}
+
+bool NetworkRobot::WaitForData() {
+ assert(kDisableTime < 1.0);
+
+ struct timeval timeout;
+ timeout.tv_sec = 0;
+ timeout.tv_usec = kDisableTime *
+ 1000.0 /*seconds to mseconds*/ *
+ 1000.0 /*mseconds to useconds*/;
+
+ fd_set fds;
+ FD_ZERO(&fds);
+ FD_SET(socket_, &fds);
+
+ int ret = select(socket_ + 1,
+ &fds, // read fds
+ NULL, // write fds
+ NULL, // exception fds (not supported)
+ &timeout);
+ if (ret == 0) {
+ // timeout
+ return false;
+ } else if (ret == 1) {
+ return true;
+ }
+ wpi_setErrnoErrorWithContext("waiting until the socket has data");
+ return false;
+}
+
+void NetworkRobot::ReceivePacket() {
+ if (!WaitForData()) return;
+
+ union {
+ sockaddr addr;
+ sockaddr_in in;
+ } sender_address;
+ int sender_address_length = sizeof(sender_address);
+ int received = recvfrom(socket_,
+ reinterpret_cast<char *>(&values_),
+ sizeof(values_),
+ 0,
+ &sender_address.addr,
+ &sender_address_length);
+ if (received == -1) {
+ if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR ||
+ errno == ETIMEDOUT) {
+ // All of these are various kinds of timing out.
+ return;
+ }
+ wpi_setErrnoErrorWithContext("recvfrom on motor value socket");
+ return;
+ }
+ wpi_assert(static_cast<size_t>(sender_address_length) >=
+ sizeof(sender_address.in));
+ if (sender_address.in.sin_addr.s_addr !=
+ expected_sender_address_.s_addr) {
+ char address[INET_ADDR_LEN];
+ inet_ntoa_b(sender_address.in.sin_addr, address);
+ char buf[64];
+ snprintf(buf, sizeof(buf), "Received packet from wrong IP %s", address);
+ wpi_setErrorWithContext(1, buf);
+ return;
+ }
+
+ if (received != sizeof(values_)) {
+ char buf[64];
+ snprintf(buf, sizeof(buf),
+ "Got packet with %zd bytes but expected %zd\n",
+ received, sizeof(values_));
+ wpi_setErrorWithContext(1, buf);
+ return;
+ }
+ if (values_.CheckHashValue()) {
+ ProcessPacket();
+ } else {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "Hash Value Is %x", values_.hash_value);
+ wpi_setErrorWithContext(1, buf);
+ return;
+ }
+}
+
+void NetworkRobot::ProcessPacket() {
+ int8_t digital_number = values_.digital_module;
+ if (digital_number != -1) {
+ if (digital_number == 0) {
+ digital_number = SensorBase::GetDefaultDigitalModule();
+ }
+ if (digital_number < 1 || digital_number > 2) {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "Got Digital Module %d",
+ static_cast<int>(digital_number));
+ wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
+ return;
+ }
+ DigitalModule *digital_module = digital_modules_[digital_number - 1];
+ if (digital_module == NULL) {
+ digital_module = digital_modules_[digital_number - 1] =
+ DigitalModule::GetInstance(digital_number);
+ for (int i = 0; i < 10; ++i) {
+ digital_module->SetPWMPeriodScale(i + 1, 0);
+ }
+ }
+
+ for (int i = 0; i < 10; ++i) {
+ digital_module->SetPWM(i + 1, values_.pwm_outputs[i]);
+ }
+
+ uint16_t old_allocated = allocated_digital_outputs_[digital_number - 1];
+ // Have to keep track of which ones we've (de)allocated as we go through in
+ // case we have trouble allocating one of them in the middle.
+ for (int i = 0; i < 16; ++i) {
+ // If we have it allocated and this packet says we shouldn't.
+ if ((old_allocated & (1 << i)) &&
+ !(values_.digital_output_enables & (1 << i))) {
+ digital_module->FreeDIO(15 - i);
+ allocated_digital_outputs_[digital_number - 1] &= ~(1 << i);
+ // If this packet says we should have it allocated and we don't.
+ } else if ((values_.digital_output_enables & (1 << i)) &&
+ !(old_allocated & (1 << i))) {
+ if (!digital_module->AllocateDIO(15 - i, false /*input*/)) return;
+ allocated_digital_outputs_[digital_number - 1] |= 1 << i;
+ }
+ }
+ wpi_assertEqual(allocated_digital_outputs_[digital_number - 1],
+ values_.digital_output_enables);
+ digital_module->SetDIOs(values_.digital_output_enables,
+ values_.digital_output_values);
+
+ if (values_.pressure_switch_channel != 0 &&
+ values_.compressor_channel != 0) {
+ digital_module->SetRelayForward(values_.compressor_channel,
+ !digital_module->GetDIO(
+ values_.pressure_switch_channel));
+ }
+ }
+
+ int8_t solenoid_number = values_.solenoid_module;
+ if (solenoid_number != -1) {
+ if (solenoid_number == 0) {
+ solenoid_number = SensorBase::GetDefaultSolenoidModule();
+ }
+ if (solenoid_number < 1 || solenoid_number > 2) {
+ char buf[64];
+ snprintf(buf, sizeof(buf), "Got Solenoid Module %d",
+ static_cast<int>(solenoid_number));
+ wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
+ return;
+ }
+ SolenoidBase *solenoid_base = solenoid_bases_[solenoid_number - 1];
+ if (solenoid_base == NULL) {
+ solenoid_base = solenoid_bases_[solenoid_number - 1] =
+ new SolenoidBase(solenoid_number);
+ }
+
+ solenoid_base->SetAll(values_.solenoid_values);
+ }
+
+ // This code can only assume that whatever is sending it values knows what
+ // state it should be in.
+ DriverStation::FMSState state = m_ds->GetCurrentState();
+ m_ds->InDisabled(state == DriverStation::FMSState::kDisabled);
+ m_ds->InAutonomous(state == DriverStation::FMSState::kAutonomous);
+ m_ds->InOperatorControl(state == DriverStation::FMSState::kTeleop);
+ m_ds->InTest(state == DriverStation::FMSState::kTestMode);
+
+ m_watchdog.Feed();
+ last_received_timestamp_ = Timer::GetPPCTimestamp();
+}
+
+void NetworkRobot::CreateSocket() {
+ wpi_assertEqual(socket_, -1);
+
+ socket_ = socket(AF_INET, SOCK_DGRAM, 0);
+ if (socket_ < 0) {
+ wpi_setErrnoErrorWithContext("Creating UDP Socket");
+ socket_ = -1;
+ return;
+ }
+
+ union {
+ sockaddr_in in;
+ sockaddr addr;
+ } address;
+ memset(&address, 0, sizeof(address));
+ address.in.sin_family = AF_INET;
+ address.in.sin_port = port_;
+ address.in.sin_addr.s_addr = INADDR_ANY;
+ if (bind(socket_, &address.addr, sizeof(address.addr)) == ERROR) {
+ wpi_setErrnoErrorWithContext("Binding Socket");
+ return;
+ }
+
+ int on = 1;
+ errno = 0;
+ if (ioctl(socket_, FIONBIO, reinterpret_cast<int>(&on)) < 0) {
+ wpi_setErrnoErrorWithContext("Setting Socket to Non-Blocking Mode");
+ return;
+ }
+}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
new file mode 100644
index 0000000..dc291cc
--- /dev/null
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobot.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_H_
+#define WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_H_
+
+#include <inetLib.h>
+
+#include "WPILib/NetworkRobot/NetworkRobotValues.h"
+#include "WPILib/RobotBase.h"
+#include "WPILib/Base.h"
+#include "WPILib/DigitalModule.h"
+#include "WPILib/ErrorBase.h"
+#include "WPILib/SolenoidBase.h"
+
+// A simple implementation of receiving motor values over UDP.
+// Deals with turning a compressor on and off at the same time.
+// You should not try to change any of the outputs on any of the modules that
+// you have this class control outside of this class.
+// This class takes care of disabling outputs when no packets are received in
+// kDisableTime and feeding the watchdog when appropriate.
+//
+// The interface consists of receiving NetworkRobotValues structs on a given UDP
+// port from a given sender address. Each time a set of motor values is
+// received, this class will update all output values.
+class NetworkRobot : public RobotBase, public ErrorBase {
+ protected:
+ // Does not take ownership of sender_address.
+ NetworkRobot(UINT16 port, const char *sender_address);
+ virtual ~NetworkRobot();
+
+ virtual void StartCompetition();
+
+ // Called when a valid packet has been received into values_.
+ // Subclasses can override if they want to do more, however they still need to
+ // call this implementation.
+ virtual void ProcessPacket();
+
+ // Called when no packet has been received for too long and it's time to stop
+ // everything.
+ // Subclasses can override if they want to do more, however they still need to
+ // call this implementation.
+ virtual void StopOutputs();
+
+ private:
+ // How long between packets we wait until we disable all of the outputs (in
+ // seconds).
+ // Must stay less than 1 second.
+ static const double kDisableTime;
+
+ // Waits for socket_ to become readable for up to kDisableTime.
+ // Returns whether or not there is readable data available.
+ bool WaitForData();
+
+ // Receives a packet and calls ProcessPacket() if it's a good one.
+ void ReceivePacket();
+
+ // Sets socket_ to an opened socket listening on port to UDP packets from
+ // sender_address.
+ void CreateSocket();
+
+ const UINT16 port_;
+ const char *const sender_address_;
+ struct in_addr expected_sender_address_;
+
+ NetworkRobotValues values_;
+ int socket_;
+
+ // Using Timer::GetPPCTimestamp() values.
+ double last_received_timestamp_;
+
+ // Ownership of the pointers stored here stays in DigitalModule.
+ DigitalModule *digital_modules_[2];
+ // This object owns pointers stored here.
+ SolenoidBase *solenoid_bases_[2];
+
+ // A bitmask of all of the digital outputs that we have currently allocated.
+ // In hardware order.
+ UINT16 allocated_digital_outputs_[2];
+
+ DISALLOW_COPY_AND_ASSIGN(NetworkRobot);
+};
+
+#endif // WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_H_
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
new file mode 100644
index 0000000..8e25983
--- /dev/null
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "WPILib/NetworkRobot/NetworkRobotValues.h"
+
+#include <string.h>
+
+namespace {
+
+uint32_t CalculateHashValue(const char *data, size_t size) {
+ uint32_t hash_value = 0;
+ for (size_t i = 0; i < size; ++i) {
+ hash_value = (((hash_value << 8) & 0xFF00) + (data[i] & 0xFF)) % 0x04C11DB7;
+ }
+ return hash_value;
+}
+
+} // namespace
+
+void NetworkRobotValues::FillInHashValue() {
+ hash_value = CalculateHashValue(reinterpret_cast<const char *>(this) +
+ sizeof(hash_value),
+ sizeof(*this) - sizeof(hash_value));
+}
+
+bool NetworkRobotValues::CheckHashValue() const {
+ return hash_value == CalculateHashValue(reinterpret_cast<const char *>(this) +
+ sizeof(hash_value),
+ sizeof(*this) - sizeof(hash_value));
+}
diff --git a/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
new file mode 100644
index 0000000..409be4b
--- /dev/null
+++ b/aos/externals/WPILib/WPILib/NetworkRobot/NetworkRobotValues.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_VALUES_H_
+#define WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_VALUES_H_
+
+#include <stdint.h>
+
+// This file needs to not have any dependencies on any other parts of WPILib so
+// that it can be #included by other code (like that which sends these values).
+
+// The structure that actually gets sent over the network.
+// All multi-byte values are sent over the network in big endian (network)
+// byte order.
+// All channel and module numbers are 1-based like usual.
+struct NetworkRobotValues {
+ // A hash value to make sure that corrupted packets don't get used.
+ // IMPORTANT: Must stay at the beginning.
+ uint32_t hash_value;
+
+ // Which digital module this packet has values for (1 or 2).
+ // 0 means the default one and -1 means to not update any one.
+ int8_t digital_module;
+ // Raw values for all 10 PWM outputs.
+ uint8_t pwm_outputs[10];
+
+ // Bitmasks for enabling digital outputs and what values to set them to.
+ // See DigitalModule::SetDIOs for which bit is which.
+ uint16_t digital_output_enables;
+ uint16_t digital_output_values;
+
+ // Channels for a presssure switch and compressor to turn on and off based on
+ // the value from it.
+ // Whatever compressor channel is sent will be updated with the value from the
+ // corresponding pressure switch channel when the packet is received.
+ // 0 means to not do anything.
+ uint8_t pressure_switch_channel, compressor_channel;
+
+ // Which solenoid module this packet has values for (1 or 2).
+ // 0 means the default one and -1 means to not update any one.
+ int8_t solenoid_module;
+ // 1 bit for each solenoid output.
+ uint8_t solenoid_values;
+
+ // Sets hash_value to the correct value for the rest of the data.
+ void FillInHashValue();
+ // Returns whether or not hash_value matches the rest of the data. Any byte
+ // order conversion must be performed ONLY on the hash_value field before
+ // calling this.
+ bool CheckHashValue() const;
+} __attribute__((packed));
+
+#endif // WPILIB_NETWORK_ROBOT_NETWORK_ROBOT_VALUES_H_
diff --git a/frc971/crio/crio.gyp b/frc971/crio/crio.gyp
index 62d88bc..731743b 100644
--- a/frc971/crio/crio.gyp
+++ b/frc971/crio/crio.gyp
@@ -29,17 +29,35 @@
],
},
{
+ 'target_name': 'dumb_main',
+ 'type': 'static_library',
+ 'sources': [
+ 'dumb_main.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ '<(AOS)/common/common.gyp:common',
+ ],
+ },
+ {
'target_name': 'FRC_UserProgram',
'type': 'shared_library',
'dependencies': [
- 'user_program'
+ 'WPILib_changes',
+ 'dumb_main',
+ ],
+ },
+ {
+ 'target_name': 'FRC_UserProgram_old',
+ 'type': 'shared_library',
+ 'dependencies': [
+ 'user_program',
],
},
{
'target_name': 'FRC_UserProgram_WithTests',
'type': 'shared_library',
'dependencies': [
- # For testing.
'<(AOS)/build/aos_all.gyp:Crio',
],
},
diff --git a/frc971/crio/dumb_main.cc b/frc971/crio/dumb_main.cc
new file mode 100644
index 0000000..ade0af1
--- /dev/null
+++ b/frc971/crio/dumb_main.cc
@@ -0,0 +1,16 @@
+#include "WPILib/NetworkRobot/NetworkRobot.h"
+#include "WPILib/RobotBase.h"
+#include "aos/common/Configuration.h"
+
+namespace frc971 {
+
+class MyRobot : public NetworkRobot {
+ public:
+ MyRobot() : NetworkRobot(static_cast<uint16_t>(::aos::NetworkPort::kMotors),
+ ::aos::configuration::GetIPAddress(
+ ::aos::configuration::NetworkDevice::kAtom)) {}
+};
+
+} // namespace frc971
+
+START_ROBOT_CLASS(::frc971::MyRobot);
diff --git a/frc971/output/AtomMotorWriter.cc b/frc971/output/AtomMotorWriter.cc
index 8431fa7..3ba3593 100644
--- a/frc971/output/AtomMotorWriter.cc
+++ b/frc971/output/AtomMotorWriter.cc
@@ -2,14 +2,12 @@
#include <string.h>
#include <unistd.h>
-#include "aos/aos_core.h"
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/messages/RobotState.q.h"
-#include "aos/atom_code/output/MotorOutput.h"
+#include "aos/atom_code/output/motor_output.h"
+#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
#include "frc971/queues/Piston.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/constants.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
#include "frc971/control_loops/shooter/shooter_motor.q.h"
#include "frc971/control_loops/index/index_motor.q.h"
@@ -26,77 +24,84 @@
namespace frc971 {
namespace output {
-class MotorWriter : public aos::MotorOutput {
+class MotorWriter : public ::aos::MotorOutput {
// Maximum age of an output packet before the motors get zeroed instead.
static const int kOutputMaxAgeMS = 20;
static const int kEnableDrivetrain = true;
- void RunIteration() {
+ virtual void RunIteration() {
+ values_.digital_module = 0;
+ values_.pressure_switch_channel = 14;
+ values_.compressor_channel = 1;
+ values_.solenoid_module = 0;
+
drivetrain.output.FetchLatest();
if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS) && kEnableDrivetrain) {
- AddMotor(TALON, 2, drivetrain.output->right_voltage / 12.0);
- AddMotor(TALON, 3, drivetrain.output->right_voltage / 12.0);
- AddMotor(TALON, 5, -drivetrain.output->left_voltage / 12.0);
- AddMotor(TALON, 6, -drivetrain.output->left_voltage / 12.0);
+ SetPWMOutput(2, drivetrain.output->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(5, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
} else {
- AddMotor(TALON, 2, 0);
- AddMotor(TALON, 3, 0);
- AddMotor(TALON, 5, 0);
- AddMotor(TALON, 6, 0);
+ DisablePWMOutput(2);
+ DisablePWMOutput(3);
+ DisablePWMOutput(5);
+ DisablePWMOutput(6);
if (kEnableDrivetrain) {
LOG(WARNING, "drivetrain not new enough\n");
}
}
shifters.FetchLatest();
- if (shifters.IsNewerThanMS(kOutputMaxAgeMS)) {
- AddSolenoid(1, shifters->set);
- AddSolenoid(2, !shifters->set);
+ if (shifters.get()) {
+ SetSolenoid(1, shifters->set);
+ SetSolenoid(2, !shifters->set);
}
wrist.output.FetchLatest();
if (wrist.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- AddMotor(TALON, 10, wrist.output->voltage / 12.0);
+ SetPWMOutput(10, wrist.output->voltage / 12.0, kTalonBounds);
} else {
- AddMotor(TALON, 10, 0);
+ DisablePWMOutput(10);
LOG(WARNING, "wrist not new enough\n");
}
shooter.output.FetchLatest();
if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- AddMotor(TALON, 4, shooter.output->voltage / 12.0);
+ SetPWMOutput(4, shooter.output->voltage / 12.0, kTalonBounds);
} else {
- AddMotor(TALON, 4, 0);
+ DisablePWMOutput(4);
LOG(WARNING, "shooter not new enough\n");
}
angle_adjust.output.FetchLatest();
if (angle_adjust.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- AddMotor(TALON, 1, -angle_adjust.output->voltage / 12.0);
+ SetPWMOutput(1, -angle_adjust.output->voltage / 12.0, kTalonBounds);
} else {
- AddMotor(TALON, 1, 0);
+ DisablePWMOutput(1);
LOG(WARNING, "angle adjust is not new enough\n");
}
index_loop.output.FetchLatest();
+ if (index_loop.output.get()) {
+ SetSolenoid(7, index_loop.output->loader_up);
+ SetSolenoid(8, !index_loop.output->loader_up);
+ SetSolenoid(6, index_loop.output->disc_clamped);
+ SetSolenoid(3, index_loop.output->disc_ejected);
+ }
if (index_loop.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- AddMotor(TALON, 8, index_loop.output->intake_voltage / 12.0);
- AddMotor(TALON, 9, index_loop.output->transfer_voltage / 12.0);
- AddMotor(TALON, 7, -index_loop.output->index_voltage / 12.0);
- AddSolenoid(7, index_loop.output->loader_up);
- AddSolenoid(8, !index_loop.output->loader_up);
- AddSolenoid(6, index_loop.output->disc_clamped);
- AddSolenoid(3, index_loop.output->disc_ejected);
+ SetPWMOutput(8, index_loop.output->intake_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(9, index_loop.output->transfer_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(7, -index_loop.output->index_voltage / 12.0, kTalonBounds);
} else {
- AddMotor(TALON, 8, 0);
- AddMotor(TALON, 9, 0);
- AddMotor(TALON, 7, 0);
+ DisablePWMOutput(8);
+ DisablePWMOutput(9);
+ DisablePWMOutput(7);
LOG(WARNING, "index not new enough\n");
}
hangers.FetchLatest();
- if (hangers.IsNewerThanMS(kOutputMaxAgeMS)) {
- AddSolenoid(4, hangers->set);
- AddSolenoid(5, hangers->set);
+ if (hangers.get()) {
+ SetSolenoid(4, hangers->set);
+ SetSolenoid(5, hangers->set);
}
}
};
@@ -104,4 +109,9 @@
} // namespace output
} // namespace frc971
-AOS_RUN(frc971::output::MotorWriter)
+int main() {
+ ::aos::Init();
+ ::frc971::output::MotorWriter writer;
+ writer.Run();
+ ::aos::Cleanup();
+}
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 4459984..b872713 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -29,8 +29,8 @@
'sources': ['AtomMotorWriter.cc'],
'dependencies': [
'<(AOS)/atom_code/output/output.gyp:motor_output',
- '<(AOS)/atom_code/messages/messages.gyp:messages',
'<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
'<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',