forked from alifahrri/biped-walking-sim
-
Notifications
You must be signed in to change notification settings - Fork 0
/
walkinthread.cpp
67 lines (57 loc) · 1.55 KB
/
walkinthread.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
#include "walkinthread.h"
#include "walkengine.h"
#include <ctime>
#include <iostream>
#include <QTime>
#include <QElapsedTimer>
#include <QSettings>
#include "math.h"
#define TORAD M_PI/180
#define RHIP_PITCH 9
#define LHIP_PITCH 3
WalkinThread::WalkinThread(){
}
WalkinThread::WalkinThread(WalkEngine *walk, Robot *robo){
running = true;
walker = walk;
robot = robo;
enableWalk = false;
desiredBodyInclination = -5.0;
goalPosChanged = false;
}
WalkinThread::~WalkinThread(){
}
void WalkinThread::setWalk(bool walk){
enableWalk = walk;
}
void WalkinThread::run(){
double dt = 0;
QElapsedTimer elapsedTimer;
qint64 begin, end;
elapsedTimer.start();
while(running){
begin = elapsedTimer.nsecsElapsed();
mutex.lock();
if(goalPosChanged){
walker->walkRequest(goalX,goalY,0);
goalPosChanged = false;
}
walker->update(dt);
mutex.unlock();
if(enableWalk){
emit ikRequest(walker->rlegik,walker->llegik,desiredBodyInclination);
// qDebug()<<"ik : "<<walker->rlegik.at(0)<<","<<walker->rlegik.at(1)<<","<<walker->rlegik.at(2)<<" "<<walker->llegik.at(0)<<","<<walker->llegik.at(1)<<","<<walker->llegik.at(2);
}
msleep(10);
end = elapsedTimer.nsecsElapsed();
dt = (double)(end - begin)/1000000000;
// qDebug()<<"loop :"<<dt;
}
}
void WalkinThread::changeGoalPos(double gx, double gy, double ga){
mutex.lock();
goalX = gx;
goalY = gy;
goalPosChanged = true;
mutex.unlock();
}