platformio.ini

[env:alksesp32]
platform = espressif32
board = alksesp32
framework = arduino

upload_speed = 921600
monitor_speed = 115200
; monitor_port = COM9

#include <Arduino.h>
#include <Wire.h>
#include "Pixy2I2C.h"
#include "time.hpp"
#include "Pixy2Line.h"

static const uint32_t i2c_freq = 400000;

Pixy2I2C pixy;
static const uint8_t Pixy_addr = 0x54;
byte L_R_light = 0;  // cervena dioda indikuje provoz

void setup() {
    Serial.begin (115200);
    Serial.print ("Starting.../n");
   
//Wire.begin
 // inicializace Pixy2
    pixy.init(Pixy_addr);
    Serial.println (pixy.changeProg("line"));
    pinMode(L_R, OUTPUT);
}

 timeout send_data { msec(500) }; // timeout zajistuje posilani dat do PC kazdych 500 ms

void loop()
{
    if (send_data) {      // if časovač send_data dosáhne nastavené hodnoty, proveď
        send_data.ack(); // nuluje časovač send_data 
        if (L_R_light == 0) L_R_light = 1; else  L_R_light = 0;
        digitalWrite(L_R, L_R_light);  // blikani LED indikuje provoz
        pixy.line.getMainFeatures();
        if (pixy.line.numVectors) pixy.line.vectors->print();
        if (pixy.line.numIntersections) pixy.line.intersections->print();
    }
}

#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;
        }
    }
}

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

#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;

void loop() {
    if(millis() > t_enc)
    {
        t_enc += 250;
        serial->print(man->encoder(0)->value());
        serial->print(' ');
        serial->println(man->encoder(7)->value());
    }
    if(serial->available()) {
        char c = serial->read();
        switch(c) {
            case 'w':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 's':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case 'a':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 'd':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case ' ':
                man->setMotors().power(LEFT_MOTOR, 0)
                                .power(RIGHT_MOTOR, 0)
                                .set();
                break;
            case 'q':
                servo.write(60);
                break;
            case 'e':
                servo.write(120);
                break;
            case '1' ... '9':
                man->encoder(7)->drive(256 * (c - '0'), 64, nullptr, nullptr);
                man->encoder(0)->drive(256 * (c - '0'), 64, nullptr, nullptr);
                break;
            default:
                serial->write(c);
                break;
        }
    }
}

#include <Arduino.h>
#include "RBControl_manager.hpp"
#include <Servo.h>

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;

void setup() {
    Serial.begin(115200);
    servo.attach(25);
    servo.write(90);
}

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

uint32_t t_enc = 0;

void loop() {
    if(millis() > t_enc)
    {
        t_enc += 250;
        Serial.print(man->encoder(0)->value());
        Serial.print(' ');
        Serial.println(man->encoder(7)->value());
    }
    if(Serial.available()) {
        char c = Serial.read();
        switch(c) {
            case 'w':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 's':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case 'a':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 'd':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case ' ':
                man->setMotors().power(LEFT_MOTOR, 0)
                                .power(RIGHT_MOTOR, 0)
                                .set();
                break;
            case 'q':
                servo.write(60);
                break;
            case 'e':
                servo.write(120);
                break;
            case '1' ... '9':
                man->motor(LEFT_MOTOR)->drive(256 * (c - '0'), 64, nullptr);
                man->motor(RIGHT_MOTOR)->drive(256 * (c - '0'), 64, nullptr); // 96 tiků na otáčku
                break;
            default:
                Serial.write(c);
                break;
        }
    }
}

main.cpp

#include <Arduino.h>
#include "RBControl_manager.hpp"
#include <Servo.h>

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;

void setup() {
    Serial.begin(115200);
    servo.attach(25);
    servo.write(90);
}

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

void loop() {
    if(Serial.available()) {
        char c = Serial.read();
        switch(c) {
            case 'w':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 's':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case 'a':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 'd':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case ' ':
                man->setMotors().power(LEFT_MOTOR, 0)
                                .power(RIGHT_MOTOR, 0)
                                .set();
                break;
            case 'q':
                servo.write(60);
                break;
            case 'e':
                servo.write(120);
                break;
            default:
                Serial.write(c);
                break;
        }
    }
}

platformio.ini

upload_speed = 921600
monitor_speed = 115200
# upload_port = /dev/ttyUSB0
# upload_port = COM7

lib_deps =  https://github.com/RoboticsBrno/RB3201-RBControl-library.git
            https://github.com/adafruit/Adafruit_TCS34725.git

build_unflags = -std=gnu++11
build_flags =
    -std=c++14
    -fmax-errors=5

#include <Arduino.h>
#include "RBControl_manager.hpp"

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;
}


void setup() {
    Serial.begin(115200);
}

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

void loop() {
    if(Serial.available()) {
        char c = Serial.read();
        switch(c) {
            case 'w':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 's':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case 'a':
                man->setMotors().power(LEFT_MOTOR, -32)
                                .power(RIGHT_MOTOR, 32)
                                .set();
                break;
            case 'd':
                man->setMotors().power(LEFT_MOTOR, 32)
                                .power(RIGHT_MOTOR, -32)
                                .set();
                break;
            case ' ':
                man->setMotors().power(LEFT_MOTOR, 0)
                                .power(RIGHT_MOTOR, 0)
                                .set();
                break;
            default:
                Serial.write(c);
                break;
        }
    }
}