diff options
| author | Clyne Sullivan <tullivan99@gmail.com> | 2016-11-01 07:40:56 -0400 |
|---|---|---|
| committer | Clyne Sullivan <tullivan99@gmail.com> | 2016-11-01 07:40:56 -0400 |
| commit | fff9060df3781b560c3f33c254cf935a2c4ea3a9 (patch) | |
| tree | 6239ff59105cbaf64dc3d7045f85506bf97f678f /vexuser.c | |
first commit
Diffstat (limited to 'vexuser.c')
| -rw-r--r-- | vexuser.c | 134 |
1 files changed, 134 insertions, 0 deletions
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));
+}
|