parser.cpp

Go to the documentation of this file.

 
#include <QByteArray>
#include <QDebug>
#include <QPoint>
 
#include <sstream>
#include <string>
 
#include "parser.hh"
 
bool Parser::parseFrame(const std::string& frame)
{
    char frameHeader = ' ';
    std::istringstream frameStream(frame);
    unsigned short int CRC16 = CRC_INIT;
    int frameLength = strlen(frame.c_str()); // length method counts \0 :(
 
    frameStream >> frameHeader;
    if(frameStream.fail() || frameHeader != 'X')
    {
        qDebug() << "Could not read frame identifier or is incorrect";
        frameStream.clear();
        return false;
    }
 
    frameStream >> _leftSpeed >> _rightSpeed;
    frameStream >> _leftAcceleration >> _rightAcceleration;
    frameStream >> _xAxis >> _yAxis;
    frameStream >> _battery;
 
    for(int i = 0; i < 200; ++i)
    {
        float angle = 0, distance = 0;
        frameStream >> angle >> distance;
        angle = (angle * 360.0) / 200.0; // stepper motor has 200 steps per revolution
        _lidarData.insert_or_assign(angle, distance);
    }
 
    frameStream >> std::hex >> _CRC16;
 
    const char* buffer = frame.c_str();
    int length = frameLength - 4; // At the end there is CRC16
 
    while(length--)
    {
        CRC16 ^= *(unsigned const char*)buffer++ << 8;
        for(int i = 0; i < 8; i++)
        {
            CRC16 = CRC16 & 0x8000 ? (CRC16 << 1) ^ CRC_POLYNOMIAL : CRC16 << 1;
        }
    }
 
    if(CRC16 != _CRC16)
    {
        qDebug() << "Calculated CRC16 different from read one";
        return false;
    }
 
    return true;
}
 
void Parser::processData()
{
    QString value;
 
    value = QString::number(_leftSpeed);
    emit changedLeftMotorSpeed(value);
 
    value = QString::number(_rightSpeed);
    emit changedRightMotorSpeed(value);
 
    value = QString::number(_leftAcceleration);
    emit changedLeftMotorAcceleration(value);
 
    value = QString::number(_rightAcceleration);
    emit changedRightMotorAcceleration(value);
 
    value = QString::number(calculateSpeedLeft(_xAxis, _yAxis));
    if(value.toFloat() > 0)
    {
        emit leftMotorDirection(1);
    }
    else if(value.toFloat() < 0)
    {
        emit leftMotorDirection(-1);
    }
    else
    {
        emit leftMotorDirection(0);
    }
    emit changedLeftMotorGSpeed(value);
 
    value = QString::number(calculateSpeedRight(_xAxis, _yAxis));
    if(value.toFloat() > 0)
    {
        emit rightMotorDirection(1);
    }
    else if(value.toFloat() < 0)
    {
        emit rightMotorDirection(-1);
    }
    else
    {
        emit rightMotorDirection(0);
    }
    emit changedRightMotorGSpeed(value);
 
    emit changedJoystick(QPoint(_xAxis, _yAxis));
 
    emit changedBattery(_battery);
 
    emit changedLidar(_lidarData);
 
    for(auto&& [angle, distance] : _lidarData)
    {
        if(distance < 300)
        {
            emit setObstacleCloseStatus();
            return;
        }
    }
 
    emit resetObstacleCloseStatus();
}
 
float Parser::calculateSpeedRight(const int& xAxis, const int& yAxis)
{
    if(_yAxis > H_JOY_LIMIT_Y && _xAxis < L_JOY_LIMIT_X)
    {
        const int speed = -30;
        return speed;
    }
    else if(_yAxis > H_JOY_LIMIT_Y && _xAxis > H_JOY_LIMIT_X)
    {
        const int speed = -5;
        return speed;
    }
    else if(_yAxis < L_JOY_LIMIT_Y && _xAxis < L_JOY_LIMIT_X)
    {
        const int speed = 30;
        return speed;
    }
    else if(_yAxis < L_JOY_LIMIT_Y && _xAxis > H_JOY_LIMIT_X)
    {
        const int speed = 5;
        return speed;
    }
    else if(_yAxis > L_JOY_LIMIT_Y && _yAxis < H_JOY_LIMIT_Y && _xAxis < H_JOY_LIMIT_X && _xAxis > L_JOY_LIMIT_X)
    {
        const int speed = 0;
        return speed;
    }
    else if(_yAxis >= L_JOY_LIMIT_Y && _yAxis <= H_JOY_LIMIT_Y && _xAxis > H_JOY_LIMIT_X)
    {
        const int speed = 0.01398 * xAxis - 27.27;
        return speed;
    }
    else if(_yAxis >= L_JOY_LIMIT_Y && _yAxis <= H_JOY_LIMIT_Y && _xAxis < L_JOY_LIMIT_X)
    {
        const int speed = -0.01554 * xAxis + 30.0;
        return speed;
    }
    else if(_yAxis > H_JOY_LIMIT_Y)
    {
        const int speed = -(0.01474 * yAxis - 30.3685);
        return speed;
    }
    else if(_yAxis < L_JOY_LIMIT_Y)
    {
        const int speed = -0.01554 * yAxis + 30.0;
        return speed;
    }
    else
    {
        const int speed = xAxis + yAxis;
        return speed;
    }
}
 
float Parser::calculateSpeedLeft(const int& xAxis, const int& yAxis)
{
    if(_yAxis > H_JOY_LIMIT_Y && _xAxis < L_JOY_LIMIT_X)
    {
        const int speed = -5;
        return speed;
    }
    else if(_yAxis > H_JOY_LIMIT_Y && _xAxis > H_JOY_LIMIT_X)
    {
        const int speed = -30;
        return speed;
    }
    else if(_yAxis < L_JOY_LIMIT_Y && _xAxis < L_JOY_LIMIT_X)
    {
        const int speed = 5;
        return speed;
    }
    else if(_yAxis < L_JOY_LIMIT_Y && _xAxis > H_JOY_LIMIT_X)
    {
        const int speed = 30;
        return speed;
    }
    else if(_yAxis > L_JOY_LIMIT_Y && _yAxis < H_JOY_LIMIT_Y && _xAxis < H_JOY_LIMIT_X && _xAxis > L_JOY_LIMIT_X)
    {
        const int speed = 0;
        return speed;
    }
    else if(_yAxis >= L_JOY_LIMIT_Y && _yAxis <= H_JOY_LIMIT_Y && _xAxis > H_JOY_LIMIT_X)
    {
        const int speed = -(0.01398 * xAxis - 27.27);
        return speed;
    }
    else if(_yAxis >= L_JOY_LIMIT_Y && _yAxis <= H_JOY_LIMIT_Y && _xAxis < L_JOY_LIMIT_X)
    {
        const int speed = -(-0.01554 * xAxis + 30.0);
        return speed;
    }
    else if(_yAxis > H_JOY_LIMIT_Y)
    {
        const int speed = -(0.01474 * yAxis - 30.3685);
        return speed;
    }
    else if(_yAxis < L_JOY_LIMIT_Y)
    {
        const int speed = -0.01554 * yAxis + 30.0;
        return speed;
    }
    else
    {
        const int speed = xAxis + yAxis;
        return speed;
    }
}