From fff9060df3781b560c3f33c254cf935a2c4ea3a9 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Tue, 1 Nov 2016 07:40:56 -0400
Subject: first commit

---
 vexuser.c | 134 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 134 insertions(+)
 create mode 100644 vexuser.c

(limited to 'vexuser.c')

diff --git a/vexuser.c b/vexuser.c
new file mode 100644
index 0000000..1088fe0
--- /dev/null
+++ b/vexuser.c
@@ -0,0 +1,134 @@
+#include <stdlib.h>
+#include <math.h>
+
+#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)   p, kVexMotor##t, kVexMotor##r
+#define MOTOR_CFG_NOIME          kVexSensorNone, 0
+#define MOTOR_CFG_IME(c)         kVexSensorIME,  kImeChannel_##c
+
+#include "ch.h"  		// needs for all ChibiOS programs
+#include "hal.h" 		// hardware abstraction layer header
+#include "vex.h"		// vex library header
+
+// motor ports
+#define mDriveFrontLeft		kVexMotor_6
+#define mDriveFrontRight	kVexMotor_9
+#define mDriveBackLeft		kVexMotor_7
+#define mDriveBackRight		kVexMotor_8
+#define mLiftLowRight		kVexMotor_5
+#define mLiftHighRight		kVexMotor_4
+#define mLiftLowLeft		kVexMotor_3
+#define mLiftHighLeft		kVexMotor_2
+#define mPickupThingy		kVexMotor_10
+#define mClawThingy			kVexMotor_1
+
+// Digi IO configuration
+static vexDigiCfg dConfig[] = {
+};
+
+static vexMotorCfg mConfig[] = {
+	{ MOTOR_CFG_MOT(mClawThingy,      393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mDriveFrontLeft,  393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mDriveFrontRight, 393T, Reversed), MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mDriveBackLeft,   393T, Reversed), MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mDriveBackRight,  393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mLiftLowRight,    393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mLiftHighRight,   393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mLiftLowLeft,     393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mLiftHighLeft,    393T, Normal),   MOTOR_CFG_NOIME },
+	{ MOTOR_CFG_MOT(mPickupThingy,    393T, Normal),   MOTOR_CFG_NOIME }
+};
+
+void vexUserSetup(void)
+{
+	vexDigitalConfigure(dConfig, DIG_CONFIG_SIZE(dConfig));
+	vexMotorConfigure(mConfig, MOT_CONFIG_SIZE(mConfig));
+}
+
+void vexUserInit(void)
+{}
+
+msg_t vexAutonomous(void* arg)
+{
+    (void)arg;
+
+    vexTaskRegister("auton");
+
+	vexMotorSet(mClawThingy, -127);
+	vexMotorSet(mPickupThingy, -64);
+	vexSleep(300);
+	vexMotorSet(mClawThingy, 0);
+	vexMotorSet(mPickupThingy, 0);
+
+	vexMotorSet(mDriveFrontLeft,  127);
+	vexMotorSet(mDriveFrontRight, 127);
+	vexMotorSet(mDriveBackLeft,   127);
+	vexMotorSet(mDriveBackRight,  127);
+	vexSleep(3000);
+	vexMotorSet(mDriveFrontLeft,  -30);
+	vexMotorSet(mDriveFrontRight, -30);
+	vexMotorSet(mDriveBackLeft,   -30);
+	vexMotorSet(mDriveBackRight,  -30);
+	vexSleep(1000);
+	vexMotorSet(mDriveFrontLeft,  0);
+	vexMotorSet(mDriveFrontRight, 0);
+	vexMotorSet(mDriveBackLeft,   0);
+	vexMotorSet(mDriveBackRight,  0);
+
+    while(1)
+        vexSleep(25);
+
+    return (msg_t)0;
+}
+
+typedef struct {
+	int x, y;
+} __attribute__ ((packed)) joy_t;
+
+int doubleButton(int btnp, int btnn, int speed);
+
+msg_t vexOperator(void* arg)
+{
+	static joy_t joyLeft, joyRight;
+
+	(void)arg;
+
+	vexTaskRegister("operator");
+
+	while(!chThdShouldTerminate()) {
+
+		// control update
+		joyLeft  = (joy_t) {vexControllerGet(Ch4), vexControllerGet(Ch3)};
+		joyRight = (joy_t) {vexControllerGet(Ch1), vexControllerGet(Ch2)};
+
+		// drive motors
+		vexMotorSet(mDriveFrontLeft,  joyLeft.y + joyLeft.x);
+		vexMotorSet(mDriveFrontRight, joyLeft.y - joyLeft.x);
+		vexMotorSet(mDriveBackLeft,   joyLeft.y + joyLeft.x);
+		vexMotorSet(mDriveBackRight,  joyLeft.y - joyLeft.x);
+
+		// lift motors
+		vexMotorSet(mLiftLowRight,  joyRight.y);
+		vexMotorSet(mLiftHighRight, joyRight.y);
+		vexMotorSet(mLiftLowLeft,   joyRight.y);
+		vexMotorSet(mLiftHighLeft,  joyRight.y);
+
+		// lift thingy
+		vexMotorSet(mPickupThingy, doubleButton(Btn5U, Btn5D, 64));
+
+		// claw thingy
+		vexMotorSet(mClawThingy, doubleButton(Btn6U, Btn6D, 127));
+
+		vexSleep(25);
+	}
+
+	return (msg_t)0;
+}
+
+
+int doubleButton(int btnp, int btnn, int speed)
+{
+	return (vexControllerGet(btnp) ? speed : (vexControllerGet(btnn) ? -speed : 0));
+}
-- 
cgit v1.2.3