"VibroHod" source code

joystick.h

#include <Arduino.h>
#include <SoftwareSerial.h>

class Joystick{
    int m_up = 0;
    int m_down = 0;
    int m_right = 0;
    int m_left = 0;
    bool m_button = false;

    float m_x = 0;
    float m_y = 0;

    long int valX = 0;
    long int valY = 0;

    bool flag = true;

    
    SoftwareSerial& m_usart;

public:
    Joystick(SoftwareSerial& ser) : m_usart(ser) {}

    void process()
    {
        if (m_usart.available())
        {
            int data = m_usart.read();
            if (data <= 100) {
                if (flag) 
                {
                    valX = data;
                    valX = valX - 50;
                    if (valX > 0) {
                        m_right = valX;
                        m_left = 0;
                    }
                    else if (valX < 0) {
                        m_right = 0;
                        m_left = valX * (-1);
                    }
                    else {
                        m_right = 0;
                        m_left = 0;
                    }
                    m_left = map(m_left, 0, 50, 0, 255);
                    m_right = map(m_right, 0, 50, 0, 255);
                    flag = false;
                }
                else {
                    valY = data;
                    valY = (valY - 50) * -1;

                    if (valY > 0) 
                    {
                        m_up = valY;
                        m_down = 0;
                    }
                    else if (valY < 0) 
                    {
                        m_up = 0;
                        m_down = valY * (-1);
                    }
                    else {
                        m_up = 0;
                        m_down = 0;
                    }
                    m_up = map(m_up, 0, 50, 0, 255);
                    m_down = map(m_down, 0, 50, 0, 255);
                    flag = true;
                }
            }
            else{
                if(data == 105){
                    m_button = 1;
                    // digitalWrite(13,HIGH);
                }
                if(data == 106){
                    m_button = 0;
                    // digitalWrite(13,LOW);
                }
            }
        }
    }


    int up() { return m_up; }
    int down() { return m_down; }
    
    int right() { return m_right; }
    int left() { return m_left; }

    bool button() { return m_button; }
};

main.cpp

#include <Arduino.h>
#include <SoftwareSerial.h>

#include "joystick.h"
#include "motor_controller.h"


SoftwareSerial BTserial(8, 9);
Joystick joy(BTserial);


void setup()
{
  pinMode(13, OUTPUT);
  pinMode(right_motor_pin, OUTPUT);
  pinMode(left_motor_pin, OUTPUT);


  Serial.begin(9600);
  BTserial.begin(9600);
}

void loop()
{
  joy.process();
  update_motors(joy.up(), joy.right(), joy.left(), joy.button());
}

motor_controller.h

#include <Arduino.h>

int left_motor_pin = 11; // Plevat`!
int right_motor_pin = 12; // Plevat`!

int cut(const int value, const int minimum, const int maximum){
    return value >= minimum ? (value <= maximum ? value : maximum) : minimum;
}

inline void update_motors(int speed, int right, int left, bool boost) {
    int left_motor_power = (speed + left) / 2; // map(speed, 0, 255, 0, 512);
    int right_motor_power = (speed + right) / 2; // map(speed, 0, 255, 0, 512);

    // 35; 36
    double coeff = sqrt(50*50 + 50 * 50) / sqrt(35*35 + 36*36);

    right_motor_power *= coeff;
    left_motor_power *= coeff;

    if (boost){
        auto addition = 255 - coeff * 255 / 2;
        // Serial.println(addition);
        
        right_motor_power += addition;
        left_motor_power += addition;        
    }

    right_motor_power = cut(right_motor_power, 0, 255);
    left_motor_power = cut(left_motor_power, 0, 255);


    analogWrite(right_motor_pin, right_motor_power);
    analogWrite(left_motor_pin, left_motor_power);

    Serial.print(left_motor_power);
    Serial.print(" | ");
    Serial.println(right_motor_power);
    
    // Serial.println();

}