blob: 07bd6b99130ae21678eb0d64440d2ca5fb1eb9a6 (
plain)
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
|
#include "claw.hpp"
#include <ch.h>
#include <hal.h>
#include <vex.h>
#include "config.h"
#include <atomic>
static std::atomic<int> clawSpeed;
static std::atomic<int> clawTime;
static char waVexClawTask[512];
msg_t vexClawTask(void *);
namespace claw {
void init(void) {
static bool running = false;
if (!running) {
clawSpeed.store(0);
clawTime.store(0);
chThdCreateStatic(waVexClawTask, 512, NORMALPRIO - 1, vexClawTask, nullptr);
running = true;
}
}
void doClaw(int speed, int time = -1) {
clawSpeed.store(speed);
clawTime.store(time);
}
void open(void) {
doClaw(-127, openTime);
}
void close(void) {
doClaw(127, closeTime);
}
void hold(void) {
doClaw(70); // inf. hold
vexSleep(closeTime); // give it time to close
}
void unhook(void) {
vexMotorSet(mClaw, 127); // can't async this
vexSleep(500);
open();
}
}
msg_t vexClawTask(void *arg)
{
(void)arg;
vexTaskRegister("claw");
while (!chThdShouldTerminate()) {
auto time = clawTime.load();
if (time != 0) {
vexMotorSet(mClaw, clawSpeed.load());
if (time > 0) {
vexSleep(time);
clawTime.store(0);
}
}
vexSleep(100);
}
return 0;
}
|
