RobotikaBrno - RBControl

Add a new message

Title

Text

[code]
#include <Arduino.h>
#include "RBControl_manager.hpp"
#include <Servo.h>
#include "nvs_flash.h"
#include "BluetoothSerial.h"

class command_parser
{
public:
	enum state_t { bad, ready, simple_command, header, st_data };

command_parser()
		: m_state(bad), m_cmd(0), m_cmd_size(0), m_size(0), m_tx_ptr(0), m_err_cnt(0)
	{
	}

void clear()
	{
		m_state = ready;
	}

uint8_t cmd() const { return m_cmd; }
	uint8_t size() const { return m_size; }
	uint8_t const * data() const { return m_rx_buffer; }
	uint16_t error_cnt() const { return m_err_cnt; }
	void clear_error_cnt() { m_err_cnt = 0; }

uint8_t push_data(uint8_t ch)
	{
		switch (m_state)
		{
		case ready:
			if (ch == 0x80)
			{
				m_state = header;
				break;
			}

if (ch > 16)
			{
				m_size = 0;
				m_cmd = ch;
				m_state = simple_command;
				break;
			}

m_state = bad;
			break;

case simple_command:
			m_state = bad;
			break;

case header:
			m_cmd_size = ch & 0x0f;
			m_cmd = ch >> 4;

m_size = 0;
			m_state = m_cmd_size == 0? ready: st_data;
			break;

case st_data:
			m_rx_buffer[m_size++] = ch;
			m_state = m_cmd_size == m_size? ready: st_data;
			break;

default:
			;
		}

if (m_state == bad)
		{
			++m_err_cnt;
			return 254;
		}

if (m_state == simple_command || m_state == ready)
			return m_cmd;

return 255;
	}
	
	template <typename T>
	bool write(const T& v)
	{
		if(m_tx_ptr >= (16 - sizeof(v)))
			return false;
		uint8_t const* begin = reinterpret_cast<uint8_t const*>(&v);
		for(const uint8_t end = m_tx_ptr + sizeof(v); m_tx_ptr != end; ++m_tx_ptr, ++begin)
			m_tx_buffer[m_tx_ptr] = *begin;
		return true;
	}
	
	template <typename Usart>
	void send(Usart& usart, const uint8_t& cmd)
	{
		usart.write(0x80);
		usart.write(((cmd & 0x0F)<<4) | m_tx_ptr);
		for(uint8_t i = 0; i != m_tx_ptr; ++i)
			usart.write(m_tx_buffer[i]);
		m_tx_ptr = 0;
	}

state_t state() const { return m_state; }

uint8_t operator[](uint8_t index) const { return m_rx_buffer[index]; }
		
	template <typename T>
	T read(uint8_t index) { return *reinterpret_cast<T*>(&m_tx_buffer[0] + index); }
		
	uint8_t* get_rx_buffer() { return m_rx_buffer; }
	uint8_t* get_buffer() { return m_rx_buffer; }
		
	uint8_t* get_tx_buffer() { return m_tx_buffer; }

protected:
	state_t m_state;

uint8_t m_cmd;
	uint8_t m_cmd_size;

uint8_t m_rx_buffer[16];
	uint8_t m_tx_buffer[16];
	uint8_t m_size;
	uint8_t m_tx_ptr;
	uint16_t m_err_cnt;
};

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

rb::Manager& rbc() 
{
    static rb::Manager m(false);  // ve výchozím stavu se motory po puštění tlačítka vypínají, false zařídí, že pojedou, dokud nedostanou další pokyn 
    return m;
}

Servo servo;

BluetoothSerial SerialBT;
Stream* serial = nullptr;

void setup() {
    Serial.begin(115200);
    if (!SerialBT.begin("RBControl")) //Bluetooth device name
    {
        Serial.println("!!! Bluetooth initialization failed!");
        serial = &Serial;
    }
    else
    {
        serial = &SerialBT;
    }
    servo.attach(25);
    servo.write(90);
}

#define LEFT_MOTOR rb::MotorId::M1
#define RIGHT_MOTOR rb::MotorId::M2

uint32_t t_enc = 0;
command_parser parser;

void loop() {
    if(millis() > t_enc)
    {
        t_enc += 250;
        serial->print(man->encoder(0)->value());
        serial->print(' ');
        serial->println(man->encoder(7)->value());
    }
    man->leds().blue(SerialBT.hasClient());
    if(serial->available()) {
        switch(parser.push_data(serial->read()))
        {
            case 1:
                if (parser.size() >= 9)
                {
                    int fwd = int16_t(parser[0] | (parser[1] << 8));
                    int cross = int16_t(parser[2] | (parser[3] << 8));
                    cross >>= 1;
                    int left = (fwd + cross) >> 8;
                    int right = (fwd - cross) >> 8;
                    Serial.print("fwd: ");
                    Serial.print(fwd);
                    Serial.print(" cross: "); 
                    Serial.print(cross);
                    Serial.print(" left: ");
                    Serial.print(left);
                    Serial.print(" right: ");
                    Serial.print(right);
                    Serial.print(" btn: ");
                    Serial.println(parser[8]);
                    man->setMotors().power(LEFT_MOTOR, left)
                                    .power(RIGHT_MOTOR, right)
                                    .set();
                    servo.write(((parser[8] & (1<<0)) != 0) ? 180 : 90);
                }
                else
                {
                    Serial.print("Wrong length: ");
                    Serial.println(parser.size());
                }
                break;
            case 254:
                parser.clear();
                Serial.println("Bad state");
                break;
            case 255:
                break;
            default:
                Serial.print("Received cmd: ");
                Serial.println(parser.cmd());
                break;
        }
    }
}
[/code]

Password