-
Notifications
You must be signed in to change notification settings - Fork 3
/
segway.h
74 lines (59 loc) · 1.35 KB
/
segway.h
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
#pragma once
#include <QObject>
#include <QTcpSocket>
#include <QTcpServer>
#include <QTimer>
#include <QSocketNotifier>
#include <QSharedPointer>
#include <trikControl/brick.h>
using namespace trikControl;
class Segway : public QObject
{
Q_OBJECT
public:
explicit Segway(QThread *guiThread);
~Segway();
protected:
void resetToZero();
void balance_control();
void buttonPressed();
signals:
private slots:
void keysEvent();
void setConnection();
void readInfoData();
void startStabilization();
void prepareSegway();
void stabilization();
void getVoltage();
private:
Brick brick;
QTcpSocket *infoSocket;
QTcpServer infoServer;
QTimer initTimer;
QTimer batteryTimer; //100ms
QTimer taskTimer; //4ms
QSharedPointer<QSocketNotifier> keysSocket;
int keysFd;
QVector<int> gyroOriginalTilts;
QVector<float> gyroOffsetTilts;
enum { INIT_MODE,
CALC_MODE,
CONTROL_MODE
} segwayState;
int averageCount; /* average count to calc gyro offset */
float args_battery;
float args_theta_m_l;
float args_theta_m_r;
float ud_err_theta;
float ud_theta_ref;
float ud_thetadot_cmd_lpf;
float ud_psi;
float ud_theta_lpf;
float cmd_forward;
float cmd_turn;
float pwm_l;
float pwm_r;
float K_F[4];
float K_I;
};