Graffik/joystickModule.cpp at master · DynamicPerception/Graffik · GitHub

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 "joystickModule.h"
#include "motionController.h"
#include <cmath>
#include <QDebug>

joystickModule::joystickModule(QObject *root, QObject *parent) :
    QObject(parent)
{
    m_pRootItem = root;
    m_maxMotorSpeed = 1;
    connect(m_pRootItem, SIGNAL(joystickMoved(int,int)), this, SLOT(joystickMoved(int,int)));
    connect(m_pRootItem, SIGNAL(sliderMoved(int)), this, SLOT(sliderMoved(int)));
    connect(m_pRootItem, SIGNAL(dampingChanged(qreal)), this, SLOT(dampingChanged(qreal)));
    connect(m_pRootItem, SIGNAL(maxJoystickSpeedChanged(qreal)), this, SLOT(maxJoystickSpeedChanged(qreal)));
}

void joystickModule::joystickMoved(int x, int y) {
    const float joystickRange = 64;
    unsigned maxSpeed = 4250 * m_maxMotorSpeed + 750;

    float speedX = (x / joystickRange) * maxSpeed;
    float speedY = (y / joystickRange) * maxSpeed;

    speedX = std::min(speedX, (float)maxSpeed);
    speedY = std::min(speedY, (float)maxSpeed);

    if(!controller.joystickMode()) {
        controller.setJoystickMode(true, true);
        controller.setWatchdogEnable(true, false);
    }

    controller.setContinuousSpeed(2, speedX, false);
    controller.setContinuousSpeed(3, speedY, false);

    if(qAbs(speedX) == 0 && qAbs(speedY) == 0)
        controller.setJoystickMode(false);
}

void joystickModule::sliderMoved(int x) {
    const float sliderRange = 128;
    unsigned maxSpeed = 4250 * m_maxMotorSpeed + 750;

    float speedX = (x / sliderRange) * maxSpeed;
    speedX = std::min(speedX, (float)maxSpeed);

    if(!controller.joystickMode()) {
        controller.setJoystickMode(true, false);
        controller.setWatchdogEnable(true, false);
    }

    controller.setContinuousSpeed(1, speedX);

    if(qAbs(speedX) < 3)
        controller.setJoystickMode(false, false);
}

void joystickModule::maxJoystickSpeedChanged(qreal pos) {
    m_maxMotorSpeed = pos;
}

void joystickModule::dampingChanged(qreal pos) {
    // Use a square relationship between accel and
    // position for better responsiveness of damping slider
    unsigned value = 24000.0 * (pos * pos) + 1000;
    for(int i = 1; i <= 3; ++i)
        controller.setMotorAcceleration((unsigned char)i, (float)value);
}