Code: Select all
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Adafruit_NeoPixel.h>
#include <ESP32Servo.h>
// Motor pins
#define IN1 12
#define IN2 14
#define IN3 27
#define IN4 26
// NeoPixel pins
#define LED_PIN 19
#define LED_COUNT 16
// Servo pin
#define SERVO_PIN 33
Adafruit_NeoPixel ring(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
Servo myServo;
enum MotorDirection { STOP, FORWARD, BACKWARD, LEFT, RIGHT };
MotorDirection currentDirection = STOP;
unsigned long lastAnimationUpdate = 0;
const unsigned long animationInterval = 25;
uint8_t ledOffset = 0;
unsigned long servoActionTime = 0;
int targetServoAngle = 90;
BLECharacteristic* pCharacteristic;
const char* SERVICE_UUID = "12345678-1234-1234-1234-25f07f20f25";
const char* CHARACTERISTIC_UUID = "abcd1234-5678-90ab-cdef-25f07f20f25";
void policeBeaconAnimation() {
if (millis() - lastAnimationUpdate >= animationInterval) {
lastAnimationUpdate = millis();
ledOffset = (ledOffset + 1) % LED_COUNT;
ring.clear();
for (int i = 0; i < LED_COUNT; i++) {
int pos = (i + ledOffset) % LED_COUNT;
if (i < 8) ring.setPixelColor(pos, ring.Color(255, 0, 0)); // Red
else ring.setPixelColor(pos, ring.Color(0, 0, 255)); // Blue
}
ring.show();
}
}
void setMotorPinsLow() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
currentDirection = STOP;
}
void moveForward() {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
currentDirection = FORWARD;
}
void moveBackward() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
currentDirection = BACKWARD;
}
void turnLeft() {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
currentDirection = LEFT;
}
void turnRight() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
currentDirection = RIGHT;
}
class MyCallbacks : public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic* pCharacteristic) override {
String value = pCharacteristic->getValue();
if (value.length() >= 7 &&
(uint8_t)value[1] == 0x01 &&
(uint8_t)value[4] == 0x02) {
uint16_t btn = ((uint8_t)value[6] << 8) | (uint8_t)value[5];
Serial.printf("Button state: 0x%04X\n", btn);
bool movement = false;
if (btn & 0x0100) { moveForward(); movement = true; }
else if (btn & 0x0200) { moveBackward(); movement = true; }
else if (btn & 0x0400) { turnLeft(); movement = true; }
else if (btn & 0x0800) { turnRight(); movement = true; }
if (btn & 0x0001) {
targetServoAngle = 180;
servoActionTime = millis();
} else if (btn & 0x0002) {
Serial.println("Servo CLOSE");
targetServoAngle = 0;
servoActionTime = millis();
}
if (!movement) setMotorPinsLow();
}
}
};
void setup() {
Serial.begin(115200);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
setMotorPinsLow(); // initialize all motor pins LOW
ring.begin();
ring.setBrightness(80);
ring.show();
myServo.attach(SERVO_PIN);
myServo.write(90); // Neutral position
BLEDevice::init("TC-ATO");
BLEServer* pServer = BLEDevice::createServer();
BLEService* pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR
);
pCharacteristic->setCallbacks(new MyCallbacks());
pCharacteristic->addDescriptor(new BLE2902());
pService->start();
BLEAdvertising* pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(true);
pAdvertising->setMinPreferred(0x06);
pAdvertising->setMinPreferred(0x12);
BLEDevice::startAdvertising();
Serial.println("Ready! BLE active. Waiting for commands...");
}
void loop() {
policeBeaconAnimation();
// Update servo position only when triggered
if (servoActionTime && millis() - servoActionTime >= 15) {
myServo.write(targetServoAngle);
servoActionTime = 0;
}
// Debug motor pins state every second
static unsigned long lastDebug = 0;
if (millis() - lastDebug > 1000) {
lastDebug = millis();
Serial.print("Motor pins states - IN1: ");
Serial.print(digitalRead(IN1));
Serial.print(", IN2: ");
Serial.print(digitalRead(IN2));
Serial.print(", IN3: ");
Serial.print(digitalRead(IN3));
Serial.print(", IN4: ");
Serial.println(digitalRead(IN4));
}
}
Code: Select all
String value = pCharacteristic->getValue();Code: Select all
std::string value ... Thanks in advance for the help,
bytebandit