/
datacollector.cpp
129 lines (109 loc) · 3.47 KB
/
datacollector.cpp
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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
#include "datacollector.h"
#define M_PI 3.14159265359
#include <myo.hpp>
#include "controller.h"
#include "armlift.h"
#include "claw.h"
#include "drive.h"
DataCollector::DataCollector()
: onArm(false), isUnlocked(false), roll_w(0), pitch_w(0), yaw_w(0), currentPose()
{
}
void DataCollector::initialize(Controller *control){
controller = control;
controlSystem = 0;
systemUnlocked = false;
}
void DataCollector::onUnpair(myo::Myo* myo, uint64_t timestamp)
{
roll_w = 0;
pitch_w = 0;
yaw_w = 0;
yaw = 0;
pitch = 0;
roll = 0;
onArm = false;
isUnlocked = false;
}
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
controlSystem = controller->getControlSystem();
systemUnlocked = controller->getSystemStatus();
if (controlSystem==2 && systemUnlocked){
roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
roll_w = static_cast<int>((roll + (float)M_PI/2.0f)/M_PI * 18);
controller->setRoll(roll_w);
} else {
pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
controller->setPitch(pitch_w);
//cout << "SPEED: " << controller->longSpeed[pitch_w] << endl;
}
if (controlSystem == 0 && systemUnlocked){
yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
controller->setYaw(yaw_w);
}
}
void DataCollector::onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
{
currentPose = pose;
if (pose != myo::Pose::unknown && pose != myo::Pose::rest) {
myo->unlock(myo::Myo::unlockHold);
if (controlSystem == 2&& systemUnlocked ){
if(pose == myo::Pose::fingersSpread){
controller->setPose(0);
}
else if(pose == myo::Pose::fist){
controller->setPose(1);
}
}
myo->notifyUserAction();
} else {
myo->unlock(myo::Myo::unlockHold);
}
}
void DataCollector::onArmSync(myo::Myo* myo, uint64_t timestamp, myo::Arm arm, myo::XDirection xDirection, float rotation,
myo::WarmupState warmupState)
{
onArm = true;
whichArm = arm;
}
void DataCollector::onArmUnsync(myo::Myo* myo, uint64_t timestamp)
{
onArm = false;
}
void DataCollector::onUnlock(myo::Myo* myo, uint64_t timestamp)
{
isUnlocked = true;
}
void DataCollector::onLock(myo::Myo* myo, uint64_t timestamp)
{
isUnlocked = false;
}
void DataCollector::print()
{
/*string controllingNow;
std::cout << '\r';
if (controlSystem == 0){
controllingNow = "drive";
} else {
controllingNow = "arm";
}
std::cout
<< '[' << controllingNow << controller->index << ": " << controller->latSpeed[controller->index] << std::string(18 - controller->index, ' ') << ']'
<< '[' << controllingNow << pitch_w << ": " << controller->longSpeed [pitch_w] << std::string(18 - pitch_w, ' ') << ']';
if (onArm) {
std::string poseString = currentPose.toString();
std::cout << '[' << (isUnlocked ? "unlocked" : "locked ") << ']'
<< '[' << poseString << std::string(14 - poseString.size(), ' ') << ']';
}*/
std::cout << std::flush;
}