1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
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);
}
|
