From d290f980aa14d5404f504009820f473eae46170a Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Thu, 16 Feb 2017 15:37:03 -0500
Subject: oh hey
---
lift.cpp | 79 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 79 insertions(+)
create mode 100644 lift.cpp
(limited to 'lift.cpp')
diff --git a/lift.cpp b/lift.cpp
new file mode 100644
index 0000000..b94f228
--- /dev/null
+++ b/lift.cpp
@@ -0,0 +1,79 @@
+#include "lift.hpp"
+
+#include <ch.h>
+#include <hal.h>
+#include <vex.h>
+#include "config.h"
+
+#include <atomic>
+
+static std::atomic<int> liftTargetLoc;
+
+static char waVexLiftTask[512];
+msg_t vexLiftTask(void *);
+
+namespace lift {
+ void init(void) {
+ static bool running = false;
+ if (!running) {
+ liftTargetLoc.store(0);
+ chThdCreateStatic(waVexLiftTask, 512, NORMALPRIO - 1, vexLiftTask, nullptr);
+ running = true;
+ }
+ }
+
+ void set(const int& target) {
+ auto t = target;
+ if (t < -10)
+ t = -10;
+ else if (t > liftMaxHeight)
+ t = liftMaxHeight;
+
+ liftTargetLoc.store(t);
+ }
+
+ void raise(void) {
+ liftTargetLoc.store(liftTargetLoc.load() + 5);
+ }
+
+ void lower(void) {
+ liftTargetLoc.store(liftTargetLoc.load() - 5);
+ }
+}
+
+msg_t vexLiftTask(void *arg)
+{
+ (void)arg;
+
+ vexTaskRegister("lift");
+
+ //static int timeout = -1;
+ while (!chThdShouldTerminate()) {
+ int actual = vexEncoderGet(kVexQuadEncoder_1);
+ int diff = liftTargetLoc.load() - actual;
+ int speed = 0;
+ if (diff != 0) {
+ /*if (timeout == -1)
+ timeout = 10;
+ else if (timeout > 0)
+ timeout--;
+ else
+ diff *= 2;*/
+
+ speed = 2 * diff;
+ } else {
+ /*timeout = -1,*/ speed = 0;
+ }
+
+ vexLcdPrintf(0, 1, "A%02d D%d S%d", actual, diff, speed);
+ vexMotorSet(mLiftLowLeft, speed);
+ vexMotorSet(mLiftHighLeft, speed);
+ vexMotorSet(mLiftLowRight, speed);
+ vexMotorSet(mLiftHighRight, speed);
+
+ vexSleep(50);
+ }
+
+ return static_cast<msg_t>(0);
+}
+
--
cgit v1.2.3