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/atom_code/input/FRCComm.h b/aos/atom_code/input/FRCComm.h
new file mode 100644
index 0000000..3d2f4c0
--- /dev/null
+++ b/aos/atom_code/input/FRCComm.h
@@ -0,0 +1,115 @@
+/*************************************************************
+ * NOTICE
+ *
+ * These are the only externally exposed functions to the
+ * NetworkCommunication library
+ *
+ * This is an implementation of FRC Spec for Comm Protocol
+ * Revision 4.5, June 30, 2008
+ *
+ * Copyright (c) National Instruments 2008. All Rights Reserved.
+ *
+ *************************************************************/
+
+#ifndef __FRC_COMM_H__
+#define __FRC_COMM_H__
+
+#include <stdint.h>
+
+typedef uint64_t UINT64;
+typedef uint32_t UINT32;
+typedef uint16_t UINT16;
+typedef uint8_t UINT8;
+typedef int8_t INT8;
+
+struct FRCCommonControlData{
+ UINT16 packetIndex;
+ union {
+ UINT8 control;
+ struct {
+ /*the order of these are flipped on the fit pc side to make it work*/
+ UINT8 fpgaChkSum :1;
+ UINT8 cRIOChkSum :1;
+ UINT8 resync : 1;
+ UINT8 fmsAttached:1;
+ UINT8 autonomous : 1;
+ UINT8 enabled : 1;
+ UINT8 notEStop : 1;
+ UINT8 reset : 1;
+ };
+ };
+ UINT8 dsDigitalIn;
+ UINT16 teamID;
+
+ char dsID_Alliance;
+ char dsID_Position;
+
+ union {
+ INT8 stick0Axes[6];
+ struct {
+ INT8 stick0Axis1;
+ INT8 stick0Axis2;
+ INT8 stick0Axis3;
+ INT8 stick0Axis4;
+ INT8 stick0Axis5;
+ INT8 stick0Axis6;
+ };
+ };
+ UINT16 stick0Buttons; // Left-most 4 bits are unused
+
+ union {
+ INT8 stick1Axes[6];
+ struct {
+ INT8 stick1Axis1;
+ INT8 stick1Axis2;
+ INT8 stick1Axis3;
+ INT8 stick1Axis4;
+ INT8 stick1Axis5;
+ INT8 stick1Axis6;
+ };
+ };
+ UINT16 stick1Buttons; // Left-most 4 bits are unused
+
+ union {
+ INT8 stick2Axes[6];
+ struct {
+ INT8 stick2Axis1;
+ INT8 stick2Axis2;
+ INT8 stick2Axis3;
+ INT8 stick2Axis4;
+ INT8 stick2Axis5;
+ INT8 stick2Axis6;
+ };
+ };
+ UINT16 stick2Buttons; // Left-most 4 bits are unused
+
+ union {
+ INT8 stick3Axes[6];
+ struct {
+ INT8 stick3Axis1;
+ INT8 stick3Axis2;
+ INT8 stick3Axis3;
+ INT8 stick3Axis4;
+ INT8 stick3Axis5;
+ INT8 stick3Axis6;
+ };
+ };
+ UINT16 stick3Buttons; // Left-most 4 bits are unused
+
+ //Analog inputs are 10 bit right-justified
+ UINT16 analog1;
+ UINT16 analog2;
+ UINT16 analog3;
+ UINT16 analog4;
+
+ UINT64 cRIOChecksum;
+ UINT32 FPGAChecksum0;
+ UINT32 FPGAChecksum1;
+ UINT32 FPGAChecksum2;
+ UINT32 FPGAChecksum3;
+
+ char versionData[8];
+};
+
+
+#endif
diff --git a/aos/atom_code/input/JoystickInput.cpp b/aos/atom_code/input/JoystickInput.cpp
new file mode 100644
index 0000000..6aeca4b
--- /dev/null
+++ b/aos/atom_code/input/JoystickInput.cpp
@@ -0,0 +1,66 @@
+#include "aos/atom_code/input/JoystickInput.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/Configuration.h"
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/messages/RobotState.q.h"
+
+namespace aos {
+
+void JoystickInput::SetupButtons() {
+ for (int i = 0; i < 4; ++i) {
+ old_buttons[i] = buttons[i];
+ }
+ buttons[0] = control_data_.stick0Buttons;
+ buttons[1] = control_data_.stick1Buttons;
+ buttons[2] = control_data_.stick2Buttons;
+ buttons[3] = control_data_.stick3Buttons;
+
+ buttons[0] |= (control_data_.enabled << (ENABLED - 9)) |
+ (control_data_.autonomous << (AUTONOMOUS - 9)) |
+ (control_data_.fmsAttached << (FMS_ATTACHED - 9));
+
+ for (int j = 0; j < 4; ++j) {
+ for (int k = 1; k <= 12; ++k) {
+ if (PosEdge(j, k)) {
+ LOG(INFO, "PosEdge(%d, %d)\n", j, k);
+ }
+ if (NegEdge(j, k)) {
+ LOG(INFO, "NegEdge(%d, %d)\n", j, k);
+ }
+ }
+ }
+ if (PosEdge(0, ENABLED)) LOG(INFO, "PosEdge(ENABLED)\n");
+ if (NegEdge(0, ENABLED)) LOG(INFO, "NegEdge(ENABLED)\n");
+ if (PosEdge(0, AUTONOMOUS)) LOG(INFO, "PosEdge(AUTONOMOUS)\n");
+ if (NegEdge(0, AUTONOMOUS)) LOG(INFO, "NegEdge(AUTONOMOUS)\n");
+ if (PosEdge(0, FMS_ATTACHED)) LOG(INFO, "PosEdge(FMS_ATTACHED)\n");
+ if (NegEdge(0, FMS_ATTACHED)) LOG(INFO, "NegEdge(FMS_ATTACHED)\n");
+}
+
+void JoystickInput::Run() {
+ ReceiveSocket sock(NetworkPort::kDS);
+ while (true) {
+ if (sock.Recv(&control_data_, sizeof(control_data_)) == -1) {
+ LOG(WARNING, "socket receive failed\n");
+ continue;
+ }
+ SetupButtons();
+ if (!robot_state.MakeWithBuilder().enabled(Pressed(0, ENABLED)).
+ autonomous(Pressed(0, AUTONOMOUS)).
+ team_id(ntohs(control_data_.teamID)).Send()) {
+ LOG(WARNING, "sending robot_state failed\n");
+ }
+ if (robot_state.FetchLatest()) {
+ char state[1024];
+ robot_state->Print(state, sizeof(state));
+ LOG(DEBUG, "robot_state={%s}\n", state);
+ } else {
+ LOG(WARNING, "fetching robot_state failed\n");
+ }
+ RunIteration();
+ }
+}
+
+} // namespace aos
+
diff --git a/aos/atom_code/input/JoystickInput.h b/aos/atom_code/input/JoystickInput.h
new file mode 100644
index 0000000..de5de04
--- /dev/null
+++ b/aos/atom_code/input/JoystickInput.h
@@ -0,0 +1,45 @@
+#ifndef AOS_INPUT_JOYSTICK_INPUT_H_
+#define AOS_INPUT_JOYSTICK_INPUT_H_
+
+#include "FRCComm.h"
+
+namespace aos {
+
+// Class for implementing atom code that reads the joystick values from the
+// cRIO.
+// Designed for a subclass that implements RunIteration to be instantiated and
+// Runed.
+class JoystickInput {
+ private:
+ uint16_t buttons[4], old_buttons[4];
+ inline uint16_t MASK(int button) {
+ return 1 << ((button > 8) ? (button - 9) : (button + 7));
+ }
+ void SetupButtons();
+ protected:
+ FRCCommonControlData control_data_;
+
+ // Constants that retrieve data when used with joystick 0.
+ static const int ENABLED = 13;
+ static const int AUTONOMOUS = 14;
+ static const int FMS_ATTACHED = 15;
+ bool Pressed(int stick, int button) {
+ return buttons[stick] & MASK(button);
+ }
+ bool PosEdge(int stick, int button) {
+ return !(old_buttons[stick] & MASK(button)) && (buttons[stick] & MASK(button));
+ }
+ bool NegEdge(int stick, int button) {
+ return (old_buttons[stick] & MASK(button)) && !(buttons[stick] & MASK(button));
+ }
+
+ virtual void RunIteration() = 0;
+ public:
+ // Enters an infinite loop that reads values and calls RunIteration.
+ void Run();
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/atom_code/input/input.gyp b/aos/atom_code/input/input.gyp
new file mode 100644
index 0000000..e77fc47
--- /dev/null
+++ b/aos/atom_code/input/input.gyp
@@ -0,0 +1,14 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'joystick',
+ 'type': 'static_library',
+ 'sources': [
+ 'JoystickInput.cpp'
+ ],
+ 'dependencies': [
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ ]
+ },
+ ],
+}