CerebellumMonitor/robot.cpp at master · DIMRobotics/CerebellumMonitor · 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
#include "robot.h"

Robot::Robot():
    pos_x(50),
    pos_y(50),
    throttle(0),
    direction(0),
    theta(0),
    renderer(QString(":/robot.svg"))
{}

void Robot::SetPosition(QCerebellum::PositionMessage& m)
{
    pos_x = m.x;
    pos_y = m.y;
    theta = m.theta * 180 / M_PI;
}

void Robot::Draw(QPainter& painter, double scale)
{
    painter.resetTransform();
    painter.scale(scale, scale);
    painter.translate(pos_x, pos_y);
    painter.rotate(theta);
    renderer.render(&painter, QRect(-31.0/2, -14, 31, 28));
}

void Robot::AddSpeed(double left, double right)
{
    v_left  += left;
    v_right += right;
}

#define sgn(d) ((0 < (d)) - ((d) < 0))

void Robot::AddDirection(double t, double d)
{
    throttle += t;
    direction += d;

    double mod_dir = throttle >= 0 ? direction : -direction;

    v_left = sgn(throttle - mod_dir) * speed;
    v_right = sgn(throttle + mod_dir) * speed;
}

void Robot::SetSpeed(int v)
{
    speed = v;
}