Subversion Repositories Code-Repo

#include "RobotControl.h"

RobotControl::RobotControl(QWidget *parent)
        : QWidget(parent)
{
        connected = false;

        connectBtn = new QPushButton();
        connectBtn->setText("Connect");
        connectBtn->setFocusPolicy(Qt::NoFocus);

        refreshBtn = new QPushButton();
        refreshBtn->setText("Refresh");
        refreshBtn->setFocusPolicy(Qt::NoFocus);

        statusLabel = new QLabel();
        statusLabel->setText("Status: Disconnected");

        speedLabel = new QLabel();
        speedLabel->setText("Speed (0-255):");

        speedBox = new QSpinBox();
        speedBox->setRange(0, 255);
        speedBox->setValue(255);
        speedBox->setSingleStep(51);
        speedBox->setFocusPolicy(Qt::NoFocus);

        portSelect = new QComboBox();
        portSelect->setFocusPolicy(Qt::NoFocus);
        refreshPorts();

        port = new QSerialPort();

        QHBoxLayout *layout = new QHBoxLayout();
        layout->addWidget(connectBtn);
        layout->addWidget(portSelect);
        layout->addWidget(refreshBtn);
        layout->addWidget(statusLabel);
        layout->addStretch();
        layout->addWidget(speedLabel);
        layout->addWidget(speedBox);
        setLayout(layout);
        
        connect(connectBtn, SIGNAL(pressed()), this, SLOT(toggleConnect()));
        connect(refreshBtn, SIGNAL(pressed()), this, SLOT(refreshPorts()));

}

RobotControl::~RobotControl()
{
        port->close();
        delete(connectBtn);
        delete(refreshBtn);
        delete(statusLabel);
        delete(portSelect);
        delete(port);
}

void RobotControl::refreshPorts() {
        ports = QSerialPortInfo::availablePorts();
        portSelect->clear();
        for (int i = 0; i < ports.size(); i++) {
                portSelect->addItem(ports[i].portName());
        }
}

void RobotControl::toggleConnect() {
        if (!connected) {
                port->setPortName(portSelect->currentText());
                if (!port->open(QIODevice::ReadWrite)) {
                        port->close();
                        statusLabel->setText("Status: Unable to Connect");
                } else {
                        port->setBaudRate(QSerialPort::Baud115200);
                        port->setDataBits(QSerialPort::Data8);
                        port->setParity(QSerialPort::NoParity);
                        port->setFlowControl(QSerialPort::NoFlowControl);
                        port->setStopBits(QSerialPort::OneStop);
                        statusLabel->setText("Status: Connected");
                        connectBtn->setText("Disconnect");
                        refreshBtn->setEnabled(false);
                        portSelect->setEnabled(false);
                        connected = true;
                }
        } else {
                port->close();
                statusLabel->setText("Status: Disconnected");
                connectBtn->setText("Connect");
                refreshBtn->setEnabled(true);
                portSelect->setEnabled(true);
                connected = false;
        }
}

void RobotControl::handleKeyPress(QKeyEvent *event) {
        if (connected) {
                if (!event->isAutoRepeat()) {
                        switch (event->key()) {
                        case Qt::Key_W:
                        case Qt::Key_Up:
                                moveForward = true;
                                break;
                        case Qt::Key_S:
                        case Qt::Key_Down:
                                moveBackward = true;
                                break;
                        case Qt::Key_A:
                        case Qt::Key_Left:
                                turnLeft = true;
                                break;
                        case Qt::Key_D:
                        case Qt::Key_Right:
                                turnRight = true;
                                break;
                        }

                        processMove();
                }
        }
}

void RobotControl::handleKeyRelease(QKeyEvent *event) {
        if (connected) {
                if (!event->isAutoRepeat()) {
                        switch (event->key()) {
                        case Qt::Key_W:
                        case Qt::Key_Up:
                                moveForward = false;
                                break;
                        case Qt::Key_S:
                        case Qt::Key_Down:
                                moveBackward = false;
                                break;
                        case Qt::Key_A:
                        case Qt::Key_Left:
                                turnLeft = false;
                                break;
                        case Qt::Key_D:
                        case Qt::Key_Right:
                                turnRight = false;
                                break;
                        }

                        processMove();
                }
        }
}

void RobotControl::processMove() {
        char buffer[4];

        // 0x01 = Reset
        // 0x02 = Left Forward
        // 0x03 = Left Backward
        // 0x04 = Right Forward
        // 0x05 = Right Backward

        char speed = speedBox->value();

        if (connected) {
                if (moveForward) {
                        if (turnLeft) {
                                buffer[0] = 0x02;
                                buffer[1] = 0x00;
                                buffer[2] = 0x04;
                                buffer[3] = speed;
                        } else if (turnRight) {
                                buffer[0] = 0x02;
                                buffer[1] = speed;
                                buffer[2] = 0x04;
                                buffer[3] = 0x00;
                        } else {
                                buffer[0] = 0x02;
                                buffer[1] = speed;
                                buffer[2] = 0x04;
                                buffer[3] = speed;
                        }
                } else if (moveBackward) {
                        if (turnLeft) {
                                buffer[0] = 0x03;
                                buffer[1] = 0x00;
                                buffer[2] = 0x05;
                                buffer[3] = speed;
                        } else if (turnRight) {
                                buffer[0] = 0x03;
                                buffer[1] = speed;
                                buffer[2] = 0x05;
                                buffer[3] = 0x00;
                        } else {
                                buffer[0] = 0x03;
                                buffer[1] = speed;
                                buffer[2] = 0x05;
                                buffer[3] = speed;
                        }
                } else if (turnLeft) {
                        buffer[0] = 0x03;
                        buffer[1] = speed;
                        buffer[2] = 0x04;
                        buffer[3] = speed;
                } else if (turnRight) {
                        buffer[0] = 0x02;
                        buffer[1] = speed;
                        buffer[2] = 0x05;
                        buffer[3] = speed;
                } else {
                        buffer[0] = 0x02;
                        buffer[1] = 0x00;
                        buffer[2] = 0x04;
                        buffer[3] = 0x00;
                }
        
                port->write(buffer, 4);
                port->flush();
        }
}