2 files changed, 104 insertions, 0 deletions
diff --git a/include/control.h b/include/control.h
new file mode 100644
index 0000000..ee9cca2
--- /dev/null
+++ b/include/control.h
@@ -0,0 +1,71 @@
+#ifndef CONTROL_H_
+#define CONTROL_H_
+
+#include <stdint.h>
+
+#include "ch.h"
+#include "hal.h"
+#include "vex.h"
+
+typedef enum {
+	BUTTON_UP,
+	BUTTON_RELEASE,
+	BUTTON_DOWN
+} button_t;
+
+struct joydata_t {
+	int Ch1 :8;
+	int Ch2 :8;
+	int Ch3 :8;
+	int Ch4 :8;
+
+	struct accel_t {
+		int y :8;
+		int x :8;
+		int z :8;
+	} __attribute__ ((packed)) accel;
+
+	char Btn5D	:1;
+	char Btn5U  :1;
+	char Btn6D  :1;
+	char Btn6U  :1;
+
+	char Reserved :4;
+
+	char Btn8D	:1;
+	char Btn8L  :1;
+	char Btn8U  :1;
+	char Btn8R  :1;
+	char Btn7D	:1;
+	char Btn7L  :1;
+	char Btn7U  :1;
+	char Btn7R  :1;
+
+	char Reserved2[2];
+
+} __attribute__ ((packed));
+
+struct Controller {
+private:
+	int index;
+	struct joydata_t* data;
+
+public:
+	Controller(int idx = 1)
+		: index(idx), data(nullptr) {}
+	~Controller(void) {}
+
+	void update(void) {
+		data = reinterpret_cast<struct joydata_t*>(vexSpiGetJoystickDataPtr(index));
+	  	data->Ch1 = vexControllerGet(Ch1);//( data->Ch1 == 0xFF ) ? 127 : data->Ch1 - 127;
+	  	data->Ch2 = vexControllerGet(Ch2);//-(( data->Ch2 == 0xFF ) ? 127 : data->Ch2 - 127);
+	  	data->Ch3 = vexControllerGet(Ch3);//-(( data->Ch3 == 0xFF ) ? 127 : data->Ch3 - 127);
+	  	data->Ch4 = vexControllerGet(Ch4);//( data->Ch4 == 0xFF ) ? 127 : data->Ch4 - 127;
+	}
+	
+	inline const struct joydata_t* operator->(void) const 
+	{ return data; }
+
+} __attribute__ ((packed));
+
+#endif // CONTROL_H_
diff --git a/include/motor.h b/include/motor.h
new file mode 100644
index 0000000..cf15088
--- /dev/null
+++ b/include/motor.h
@@ -0,0 +1,33 @@
+#ifndef MOTOR_H_
+#define MOTOR_H_
+
+#include "ch.h"
+#include "hal.h"
+#include "vex.h"
+
+typedef enum {
+	/* 1 */  mClawThingy = kVexMotor_1,
+	/* 2 */  mLiftHighLeft,
+	/* 3 */  mLiftLowLeft,
+	/* 4 */  mLiftHighRight,
+	/* 5 */  mLiftLowRight,
+	/* 6 */  mDriveFrontLeft,
+	/* 7 */  mDriveBackLeft,
+	/* 8 */  mDriveBackRight,
+	/* 9 */  mDriveFrontRight,
+	/* 10 */ mPickupThingy,
+} motor_port_t;
+
+constexpr const tVexImeChannels iLiftLowLeft   = kImeChannel_1;
+constexpr const tVexImeChannels iLiftHighLeft  = kImeChannel_2;
+constexpr const tVexImeChannels iLiftLowRight  = kImeChannel_3;
+constexpr const tVexImeChannels iLiftHighRight = kImeChannel_4;
+
+#define DIGI_CFG_DIGI_OUT(p) { kVexDigital_##p, kVexSensorDigitalOutput, kVexConfigOutput, 0 }
+#define DIGI_CFG_DIGI_IN(p)  { kVexDigital_##p, kVexSensorDigitalInput,  kVexConfigInput,  0 }
+
+#define MOTOR_CFG_MOT(p, t, r)   (tVexMotor)p, kVexMotor##t, kVexMotor##r
+#define MOTOR_CFG_NOIME          kVexSensorNone, 0
+#define MOTOR_CFG_IME(c)         kVexSensorIME,  c
+
+#endif // MOTOR_H_