Started BLE

This commit is contained in:
vanarebane 2025-02-24 16:42:12 +02:00
parent ed9c1577d2
commit 83eeabd62c
8 changed files with 282 additions and 19 deletions

42
.vscode/settings.json vendored
View File

@ -9,4 +9,46 @@
"editor.formatOnType": false,
"editor.formatOnSave": true,
"editor.formatOnSaveMode": "file",
"files.associations": {
"array": "cpp",
"*.tcc": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"deque": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"istream": "cpp",
"limits": "cpp",
"memory": "cpp",
"new": "cpp",
"ostream": "cpp",
"numeric": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"cinttypes": "cpp",
"utility": "cpp",
"typeinfo": "cpp"
},
}

View File

@ -119,10 +119,11 @@ static PB_SmartKnobConfig configs[] = {
},
};
InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) :
InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, BLETask& ble_task, DisplayTask* display_task) :
Task("Interface", 3000, 1, task_core),
stream_(),
motor_task_(motor_task),
ble_task_(ble_task),
display_task_(display_task),
plaintext_protocol_(stream_, motor_task_),
proto_protocol_(stream_, motor_task_) {

View File

@ -6,6 +6,7 @@
#include "display_task.h"
#include "logger.h"
#include "motor_task.h"
#include "network/ble_task.h"
#include "serial/serial_protocol_plaintext.h"
#include "serial/serial_protocol_protobuf.h"
#include "serial/uart_stream.h"
@ -15,7 +16,7 @@ class InterfaceTask : public Task<InterfaceTask>, public Logger {
friend class Task<InterfaceTask>; // Allow base Task to invoke protected run()
public:
InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task);
InterfaceTask(const uint8_t task_core, MotorTask& motor_task, BLETask& ble_task, DisplayTask* display_task);
virtual ~InterfaceTask() {};
void log(const char* msg) override;
@ -26,6 +27,7 @@ class InterfaceTask : public Task<InterfaceTask>, public Logger {
private:
UartStream stream_;
MotorTask& motor_task_;
BLETask& ble_task_;
DisplayTask* display_task_;
char buf_[64];

View File

@ -3,6 +3,7 @@
#include "display_task.h"
#include "interface_task.h"
#include "motor_task.h"
#include "network/ble_task.h"
#if SK_DISPLAY
static DisplayTask display_task(0);
@ -12,8 +13,9 @@ static DisplayTask* display_task_p = nullptr;
#endif
static MotorTask motor_task(1);
static BLETask ble_task(1);
InterfaceTask interface_task(0, motor_task, display_task_p);
InterfaceTask interface_task(0, motor_task, ble_task, display_task_p);
void setup() {
#if SK_DISPLAY
@ -24,9 +26,13 @@ void setup() {
motor_task.addListener(display_task.getKnobStateQueue());
#endif
ble_task.setLogger(&interface_task);
ble_task.begin();
motor_task.setLogger(&interface_task);
motor_task.begin();
interface_task.begin();
// Free up the Arduino loop task
vTaskDelete(NULL);

View File

@ -13,10 +13,15 @@
// ####
// Hardware-specific motor calibration constants.
// Run calibration once at startup, then update these constants with the calibration results.
static const float ZERO_ELECTRICAL_OFFSET = 0.65;
static const float ZERO_ELECTRICAL_OFFSET = 4.7;
static const Direction FOC_DIRECTION = Direction::CW;
static const int MOTOR_POLE_PAIRS = 7;
// static const float ZERO_ELECTRICAL_OFFSET = 0.65;
// static const Direction FOC_DIRECTION = Direction::CW;
// static const int MOTOR_POLE_PAIRS = 7;
// static const float ZERO_ELECTRICAL_OFFSET = 3.42; // Sweet spot 0.65
// static const Direction FOC_DIRECTION = Direction::CCW;
// static const int MOTOR_POLE_PAIRS = 7;
@ -78,7 +83,7 @@ void MotorTask::run() {
motor.init();
encoder.update();
delay(10);
delayMicroseconds(1000);
motor.pole_pairs = MOTOR_POLE_PAIRS;
motor.initFOC(ZERO_ELECTRICAL_OFFSET, FOC_DIRECTION);
@ -147,12 +152,12 @@ void MotorTask::run() {
motor.move(strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
delayMicroseconds(1000);
}
motor.move(-strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
delayMicroseconds(1000);
}
motor.move(0);
motor.loopFOC();
@ -224,7 +229,7 @@ void MotorTask::run() {
motor.monitor();
delay(1);
delayMicroseconds(1000);
}
}
@ -290,19 +295,19 @@ void MotorTask::calibrate() {
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
motor.move(a);
delay(1);
delayMicroseconds(1000);
}
float start_sensor = encoder.getAngle();
for (; a < 3 * _2PI; a += 0.01) {
encoder.update();
motor.move(a);
delay(1);
delayMicroseconds(1000);
}
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
delay(1);
delayMicroseconds(1000);
}
float end_sensor = encoder.getAngle();
@ -336,12 +341,12 @@ void MotorTask::calibrate() {
for (; a < destination; a += 0.03) {
encoder.update();
motor.move(a);
delay(1);
delayMicroseconds(1000);
}
log("pause..."); // Let momentum settle...
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
delay(1);
delayMicroseconds(1000);
}
log("Measuring...");
@ -350,12 +355,12 @@ void MotorTask::calibrate() {
for (; a < destination; a += 0.03) {
encoder.update();
motor.move(a);
delay(1);
delayMicroseconds(1000);
}
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
motor.move(a);
delay(1);
delayMicroseconds(1000);
}
end_sensor = motor.sensor_direction * encoder.getAngle();
motor.voltage_limit = 0;
@ -374,7 +379,7 @@ void MotorTask::calibrate() {
snprintf(buf_, sizeof(buf_), "Pole pairs set to %d", measured_pole_pairs);
log(buf_);
delay(1000);
delayMicroseconds(1000000);
// #### Determine mechanical offset to electrical zero
@ -390,7 +395,7 @@ void MotorTask::calibrate() {
delay(100);
for (uint8_t i = 0; i < 100; i++) {
encoder.update();
delay(1);
delayMicroseconds(1000);
}
float real_electrical_angle = _normalizeAngle(a);
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
@ -407,7 +412,7 @@ void MotorTask::calibrate() {
delay(100);
for (uint8_t i = 0; i < 100; i++) {
encoder.update();
delay(1);
delayMicroseconds(1000);
}
float real_electrical_angle = _normalizeAngle(a);
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);

View File

@ -0,0 +1,133 @@
#if SK_BLE
#include "ble_task.h"
BLETask::BLETask(const uint8_t task_core) : Task("BLE", 2500, 1, task_core) {
queue_ = xQueueCreate(5, sizeof(Message));
assert(queue_ != NULL);
}
BLETask::~BLETask() {}
bool deviceConnected = false;
class KnobServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
};
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
void BLETask::run() {
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVICE_UUID "0000340f-0000-1000-8000-00805f9b34fb"
#define CHARACTERISTIC_UUID "000042ff-0000-1000-8000-00805f9b34fb"
// Create the BLE Device
BLEDevice::init("SmartKnob_0123");
// Create the BLE Server
pServer = BLEDevice::createServer();
pServer->setCallbacks(new KnobServerCallbacks());
// Create the BLE Service
BLEService *pService = pServer->createService(SERVICE_UUID);
// Create a BLE Characteristic
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
// https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
// Create a BLE Descriptor
pCharacteristic->addDescriptor(new BLE2902());
// Start the service
pService->start();
// Start advertising
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter
BLEDevice::startAdvertising();
while (1) {
// Check queue for pending requests from other tasks
Message message;
if (xQueueReceive(queue_, &message, 0) == pdTRUE) {
switch (message.message_type) {
case MessageType::READ:{
break;
}
case MessageType::WRITE: {
break;
}
case MessageType::NOTIFY: {
break;
}
case MessageType::INDICATE: {
break;
}
}
}
// notify changed value
if (deviceConnected) {
pCharacteristic->setValue((uint8_t*)&test_value, 4);
pCharacteristic->notify();
test_value++;
delay(1000);
//delay(3); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
}
// disconnecting
if (!deviceConnected && oldDeviceConnected) {
delay(500); // give the bluetooth stack the chance to get things ready
pServer->startAdvertising(); // restart advertising
log("BLE start advertising");
oldDeviceConnected = deviceConnected;
}
// connecting
if (deviceConnected && !oldDeviceConnected) {
// do stuff here on connecting
oldDeviceConnected = deviceConnected;
}
}
}
void BLETask::addListener(QueueHandle_t queue) {
listeners_.push_back(queue);
}
void BLETask::publish(const PB_SmartKnobState& state) {
for (auto listener : listeners_) {
xQueueOverwrite(listener, &state);
}
}
void BLETask::setLogger(Logger* logger) {
logger_ = logger;
}
void BLETask::log(const char* msg) {
if (logger_ != nullptr) {
logger_->log(msg);
}
}
#endif

View File

@ -0,0 +1,70 @@
#pragma once
#if SK_BLE
#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Preferences.h>
#include "../logger.h"
#include "../task.h"
#include "../proto_gen/smartknob.pb.h"
enum class MessageType {
READ,
WRITE,
NOTIFY,
INDICATE,
};
struct Message {
MessageType message_type;
union MessageData {
uint8_t unused;
PB_SmartKnobConfig config;
};
MessageData data;
};
class BLETask : public Task<BLETask> {
friend class Task<BLETask>; // Allow base Task to invoke protected run()
public:
BLETask(const uint8_t task_core);
~BLETask();
// void setConfig(const PB_SmartKnobConfig& config);
// void playHaptic(bool press);
// void runCalibration();
void addListener(QueueHandle_t queue);
void setLogger(Logger* logger);
friend class KnobServerCallbacks;
protected:
void run();
private:
QueueHandle_t queue_;
Logger* logger_;
std::vector<QueueHandle_t> listeners_;
char buf_[72];
// BLE Setup
BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
// bool BLE_CONNECTED;
bool oldDeviceConnected = false;
uint32_t test_value = 0;
void publish(const PB_SmartKnobState& state);
// void calibrate();
// void checkSensorError();
void log(const char* msg);
};
#endif

View File

@ -50,12 +50,13 @@ lib_deps =
build_flags =
${base_config.build_flags}
-DSK_BLE=1
-DSK_DISPLAY=1
-DSK_LEDS=1
-DNUM_LEDS=8
-DSENSOR_MT6701=1
-DSK_STRAIN=1
-DSK_INVERT_ROTATION=1
-DSK_INVERT_ROTATION=0
-DSK_ALS=1
-DPIN_UH=25 #26
@ -75,6 +76,9 @@ build_flags =
-DPIN_LED_DATA=7
-DPIN_LCD_BACKLIGHT=19
-DPIN_STRAIN_DO=38
-DPIN_STRAIN_SCK=2
-DDESCRIPTION_FONT=Roboto_Thin_24
-DDESCRIPTION_Y_OFFSET=20
-DVALUE_OFFSET=30