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"

#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;
        }
    }
}
[/code]

Password