Simulating a joystick
-
hello, i am working on a project, my aim is as follows, i get 2 joyistick data via "uart". I process and filter this data and I have 0 - 150 data (for x and y). I'm doing this job on the C++ side. I am accessing this data from my QML file as I generate the x and y values. Now there is x and y data on the QML side. I made two nested rectangles of this data. I made the rectangles I created take the shape of a circle by giving them a radius. I'm putting these circles together, the big circle is h:150 w:150 the smaller one is h:20 w:20. My aim is to move the circle in the interior according to the incoming x and y states. it works up to this point. but the data coming from the joystick (0,0), (0,150), (150, 0) (150, 150) ... the circle in the inner part passes the outer circle. To avoid this situation, I first subtract the x and y values from the radius and add their squares, and if they are not greater than the square of the radius, I get the same, but if it is, what should I do? I use the "atan2" function to get the x and y data and get a radian, put it in the sin and cos data of this radian and multiply with the radius, but still got the wrong result. The reason for getting wrong results is because the center of the circle is in the upper left corner. What algorithm should I write?
code:#include "SerialComm.hpp" #include <QSerialPortInfo> #define VOLTAGE_DEBUG // #define DEBUG Serial::Serial(): state(0), count(0), voltage(0), cam_value(0){ port_ = new QSerialPort; connectionTimer = new QTimer(this); connectionTimer->setInterval(100); connectionTimer->start(); raw_data.resize(V_SIZE); stable_data.resize(V_SIZE); joy_offset.resize(JOY_ALL); initializationData(RESET_ALL); checkDeviceConnection(); connect(port_, &QSerialPort::readyRead, this, &Serial::readBytes); connect(port_, &QSerialPort::errorOccurred, this, [this]() { connectionTimer->start(); }); connect(connectionTimer, &QTimer::timeout, this, &Serial::checkDeviceConnection); } Serial::~Serial(){ port_->close(); connectionTimer->stop(); delete connectionTimer; delete port_; } void Serial::checkDeviceConnection(){ if(port_->isOpen()) { port_->close(); } for(const QSerialPortInfo port_list : QSerialPortInfo::availablePorts()){ if((port_list.manufacturer() == "FTDI") || (port_list.description() == "Arduino Mega 2560") ){ port_->setPortName(port_list.portName()); port_->setBaudRate(BAUDRATE); port_->setDataBits(QSerialPort::Data8); start_com = true; while(port_->open(QIODevice::ReadOnly)){ qDebug() << "ReadOnly"; } connectionTimer->stop(); } #ifdef DEBUG qDebug() << "-----------------------------------------------------"; qDebug() << "portName: " << port_list.portName(); qDebug() << "systemLocation: " << port_list.systemLocation(); qDebug() << "description: " << port_list.description(); qDebug() << "manufacturer: " << port_list.manufacturer(); qDebug() << "serialNumber: " << port_list.serialNumber(); qDebug() << "vendorIdentifier: " << port_list.vendorIdentifier(); qDebug() << "productIdentifier: " << port_list.productIdentifier(); #endif } } void Serial::readBytes(){ while(port_->bytesAvailable()){ uint8_t cur_byte; port_->read((char*)&cur_byte, 1); if(state == 0){ if((cur_byte == HEADER) && (prev_byte == FOOTHER)){ rx_buffer[state++] = cur_byte; } else { state = 0; } }else if( state < HEADER_LEN + PAYLOAD_LEN){ rx_buffer[state++] = cur_byte; }else if(state < HEADER_LEN + FOOTHER_LEN + PAYLOAD_LEN){ state = 0; prev_byte = cur_byte; if(cur_byte == FOOTHER){ raw_data[0] = (static_cast<int16_t>(rx_buffer[1] & 0x01)); raw_data[1] = (static_cast<int16_t>((rx_buffer[1]>>1) & 0x01)); raw_data[2] = (static_cast<int16_t>((rx_buffer[1]>>2) & 0x01)); raw_data[3] = (static_cast<int16_t>((rx_buffer[1]>>3) & 0x01)); raw_data[4] = (static_cast<int16_t>((rx_buffer[1] >> 4) | ((rx_buffer[2] << 4) & 0x03FF))); raw_data[5] = (static_cast<int16_t>((rx_buffer[2] >> 6) | ((rx_buffer[3] << 2) & 0x03FF))); raw_data[6] = (static_cast<int16_t>((rx_buffer[4]) | ((rx_buffer[5] << 8) & 0x03FF))); raw_data[7] = (static_cast<int16_t>((rx_buffer[5] >> 2) | ((rx_buffer[6] << 6) & 0x03FF))); raw_data[8] = (static_cast<int16_t>((rx_buffer[6] >> 4) | ((rx_buffer[7] << 4) & 0x03FF))); raw_data[9] = (static_cast<int16_t>((rx_buffer[7] >> 6) | ((rx_buffer[8] << 2) & 0x03FF))); }else{ state = 0; } }else { state = 0; } prev_byte = cur_byte; } } void Serial::bufferCopy(QVector<int>& data){ std::copy(raw_data.begin(), raw_data.end(), stable_data.begin()); stable_data[QGC_VOL] = voltageFilter((float)raw_data[QGC_VOL]); stable_data[QGC_CAM] = camSliderFilter(raw_data[QGC_CAM]); stable_data[QGC_RX] = joyDataFilter(raw_data[4], JOY_RX); stable_data[QGC_RY] = joyDataFilter(raw_data[5], JOY_RY); stable_data[QGC_LX] = joyDataFilter(raw_data[8], JOY_LX); stable_data[QGC_LY] = joyDataFilter(raw_data[6], JOY_LY); std::copy(stable_data.begin(), stable_data.end(), data.begin()); #ifdef DEBUG qDebug() << "Genel:" << raw_data[4] << raw_data[5] << raw_data[6] << raw_data[8]; qDebug() << "Data: " << stable_data[QGC_RX] << stable_data[QGC_RY] << stable_data[QGC_LX] << stable_data[QGC_LY]; #endif } int Serial::voltageFilter(float new_data){ #ifdef VOLTAGE_DEBUG qDebug() << "voltage raw_data: " << new_data; #endif new_data = filter[5].kalman(new_data); if(new_data < 610.0 || new_data > 860.0) { return -1; } new_data = map(new_data, 610.0, 860.0, 0.0, 100.0); if(filter[6].getKalmanDataOld() == 0){ filter[6].setKalmanDataOld(new_data); } filter[6].setQR(0.005, 0.995); return (int)filter[6].kalman(new_data); } int Serial::camSliderFilter(float new_data){ #ifdef CAM_SLIDER_DEBUG qDebug() << "Cam slider raw_data: " << new_data; #endif new_data = filter[4].kalman(new_data); if(new_data > (512 + OFFSET_CAM) ) { return cam_value += 5; } else if(new_data < (512 - OFFSET_CAM)) { return cam_value -= 5; } new_data = constrain(new_data, 0.0, 1023.0); return (int)cam_value; } void Serial::calibrateJoy(){ if(count == CALIBRATE_COUNT){ joy_offset[JOY_RX] /= CALIBRATE_COUNT; joy_offset[JOY_RY] /= CALIBRATE_COUNT; joy_offset[JOY_LX] /= CALIBRATE_COUNT; joy_offset[JOY_LY] /= CALIBRATE_COUNT; calibrated = false; }else if(calibrated){ joy_offset[JOY_RX] += filter[JOY_RX].kalman(constrain(raw_data[4], 0.0, 1023.0)); joy_offset[JOY_RY] += filter[JOY_RY].kalman(constrain(raw_data[5], 0.0, 1023.0)); joy_offset[JOY_LX] += filter[JOY_LX].kalman(constrain(raw_data[8], 0.0, 1023.0)); joy_offset[JOY_LY] += filter[JOY_LY].kalman(constrain(raw_data[6], 0.0, 1023.0)); } count++; } int Serial::joyDataFilter(float new_data, JoyData_e joy_num){ new_data = constrain(new_data, JOY_MIN_OFFSET, JOY_MAX_OFFSET); new_data = filter[joy_num].kalman(new_data); if((new_data > (joy_offset[joy_num] - JOY_OFFSET)) && (new_data < (joy_offset[joy_num] + JOY_OFFSET))){ new_data = 512; }else if(new_data <= (joy_offset[joy_num] - JOY_OFFSET)){ new_data = map(new_data, JOY_MIN_OFFSET, joy_offset[joy_num] - JOY_OFFSET, 0.0, 511.0); }else{ new_data = map(new_data, joy_offset[joy_num] + JOY_OFFSET, JOY_MAX_OFFSET, 513.0, 1023.0); } new_data = map(new_data, 0, 1023, 0.0, 150.0); return (int)new_data; } void Serial::initializationData(DataReset_e n_data){ switch (n_data){ case RAW_DATA_R: std::fill(raw_data.begin(), raw_data.end(), 0); break; case STABLE_DATA_R: std::fill(stable_data.begin(), stable_data.end(), 0); break; case JOY_OFFSET_R: std::fill(joy_offset.begin(), joy_offset.end(), 512); break; case JOY_OFFSET_Z: std::fill(joy_offset.begin(), joy_offset.end(), 0); calibrated = true; count = 0; break; case RESET_ALL: std::fill(raw_data.begin(), raw_data.end(), 0); std::fill(stable_data.begin(), stable_data.end(), 0); std::fill(joy_offset.begin(), joy_offset.end(), 512); break; default: break; } }void MainRos::updateData(){ serial_comm.bufferCopy(m_data); joyCallback(m_data); m_data[QGC_RX] = map(m_data[QGC_RX], 0.0, 150.0, 150.0, 0.0); m_data[QGC_RY] = map(m_data[QGC_RY], 0.0, 150.0, 0.0, 150.0); m_data[QGC_LX] = map(m_data[QGC_LX], 0.0, 150.0, 150.0, 0.0); m_data[QGC_LY] = map(m_data[QGC_LY], 0.0, 150.0, 0.0, 150.0); axisProtection(m_data[QGC_LX], m_data[QGC_LY], true); // axisProtection(m_data); setData(m_data); } void MainRos::axisProtection(float y, float x, bool flag){ x = abs(x - 75); y = abs(y - 75); if((x * x + y * y) > 75 * 75){ double angleRadians = atan2f(x, y); m_data[QGC_LY] = 75 * sinf(angleRadians); m_data[QGC_LX] = 75 * cosf(angleRadians); } // float tan_derece = 45; // m_data[QGC_RX] = map(m_data[QGC_RX], 0.0, 150.0, 150.0, 0.0); // m_data[QGC_RY] = map(m_data[QGC_RY], 0.0, 150.0, 0.0, 150.0); // m_data[QGC_LX] = map(m_data[QGC_LX], 0.0, 150.0, 150.0, 0.0); // m_data[QGC_LY] = map(m_data[QGC_LY], 0.0, 150.0, 0.0, 150.0); // if(sqrt(m_data[QGC_RX] * m_data[QGC_RX] + m_data[QGC_RY] * m_data[QGC_RY]) > 75.0){ // double angleRadians = std::atan2(m_data[QGC_RX], m_data[QGC_RY]); // double angleDegrees = angleRadians * (180.0 / M_PI); // m_data[QGC_RX] = 75.0 * std::sin(angleRadians); // m_data[QGC_RY] = 75.0 * std::cos(angleRadians); // } // if(sqrt(m_data[QGC_RX] * m_data[QGC_RX] + m_data[QGC_RY] * m_data[QGC_RY]) > 75.0){ // double angleRadians = std::atan2(m_data[QGC_LX], m_data[QGC_LY]); // double angleDegrees = angleRadians * (180.0 / M_PI); // m_data[QGC_RX] = 75.0 * std::sin(angleRadians); // m_data[QGC_RY] = 75.0 * std::cos(angleRadians); // } // qDebug() << m_data[QGC_RX] << m_data[QGC_RY] << m_data[QGC_LX] << m_data[QGC_LY]; }qml:
Rectangle{ id:left_joy_rec anchors.top:left_top_slider.bottom anchors.topMargin:45 anchors.left: parent.left anchors.leftMargin: 80 width: joy_size height:joy_size radius:joy_size/2 clip: true color:"#D9D9D9" Rectangle{ id:line1 anchors.centerIn:parent width: parent.width height: 2 color: "black" } Rectangle{ id:line2 anchors.centerIn:parent width: 2 height: parent.height color: "black" } Rectangle{ id:left_joy width: joy_point_size height:joy_point_size radius:joy_point_size visible: true x: main_ros.data[6] - 10 y: main_ros.data[8] - 10 color:/*"#222222"*/ "red" } } Rectangle{ id:right_joy_rec anchors.top:left_top_slider.bottom anchors.topMargin:45 anchors.right: parent.right anchors.rightMargin: 80 width: joy_size height:joy_size radius:joy_size/2 clip: true color:"#D9D9D9" Rectangle{ id:line3 anchors.centerIn:parent width: parent.width height: 2 color: "black" } Rectangle{ id:line4 anchors.centerIn:parent width: 2 height: parent.height color: "black" } Rectangle{ id:right_joy width: joy_point_size height:joy_point_size radius:joy_point_size visible: true x: main_ros.data[4] - 10 y: main_ros.data[5] - 10 color:/*"#222222"*/ "red" } } -
S serkan_tr has marked this topic as solved on