دربازکن مکانیکی هوشمند با موتور براشلس و ESP32

/*
* ERAD2.1 door unlocker example program for ESP32S3-based wireless BLDC motor driver. Check it out at roboticworx.io!
*/
#include <SimpleFOC.h>
#include <SPI.h>
#include <esp_now.h>
#include <WiFi.h>
/* Pins */
// Define the BLDC motor pins
#define BLDC_PWM_UH_GPIO 7
#define BLDC_PWM_UL_GPIO 8
#define BLDC_PWM_VH_GPIO 9
#define BLDC_PWM_VL_GPIO 10
#define BLDC_PWM_WH_GPIO 11
#define BLDC_PWM_WL_GPIO 12
// TMC6200 / gate driver pins
#define DRV0_CSN_PIN 14
#define DRV1_SCK_PIN 13
// Driver enable + user LED
#define ENABLE_PIN 17
#define BLINK_PIN 38
// Encoder (AS5048A) pins
#define AS5_CLK_PIN 15 // SCK
#define AS5_MISO_PIN 16 // MISO
#define AS5_MOSI_PIN 18 // MOSI
#define AS5_CSN_PIN 21 // CS (chip select)
// Motor mechanical configuration
// CHANGE THIS: number of permanent magnets in YOUR motor divided by 2
#define POLE_PAIRS 7
/* Globals */
volatile bool toggle_request = false;
volatile unsigned int cmd_received = 0;
bool pos_state = false; // false -> pos A, true -> pos B
// Callback that triggers when data is received
void receive_cb(const esp_now_recv_info_t *info, const uint8_t *data, int len)
{
// Optionally print the sender MAC (PolyCast5)
Serial.print("From ");
for (int i = 0; i < 6; i++) {
Serial.printf("%02X", info->src_addr[i]);
if (i < 5) Serial.print(":");
}
Serial.print(" | ");
// The received data will be in the form "PolyCast5_Command_Value: %u" for security purposes
// We need to extract the %u (unsigned int) command
#define MAX_BUF_SIZE 65 // Maximum number of bytes the data could be
char buf[MAX_BUF_SIZE]; // Create a buffer to store the data string
// If the received data length is less than the max buffer size, make len the new buffer size
size_t copy_len = len < MAX_BUF_SIZE - 1 ? len : MAX_BUF_SIZE - 1;
memcpy(buf, data, copy_len); // Copy len bytes of the data into the buffer
buf[copy_len] = '\0'; // Null-terminate the string
// Try to parse "PolyCast5_Command_Value: %u" out of the string
if (sscanf(buf, "PolyCast5_Command_Value: %u", &cmd_received) == 1) { // If success -> data is valid and the command is now stored in cmd_received
Serial.print("Command: ");
Serial.println(cmd_received); // The command received
}
else { // Data is not valid
Serial.print("Unexpected data: ");
Serial.println(buf);
}
if (cmd_received != 0) {
toggle_request = true; // Request a toggle once
}
}
unsigned long previousMillis = 0;
bool blinkState = true;
// Angle set point variable
float target_angle = 0; // Default target angle (radians)
// Encoder debug print timing (so we don't spam Serial and slow down FOC)
static unsigned long lastPrintMillis = 0;
/* Motor / Driver / Sensor Instances */
// BLDC motor and driver instance
BLDCMotor motor = BLDCMotor(POLE_PAIRS);
BLDCDriver6PWM driver = BLDCDriver6PWM(
BLDC_PWM_UH_GPIO, BLDC_PWM_UL_GPIO,
BLDC_PWM_VH_GPIO, BLDC_PWM_VL_GPIO,
BLDC_PWM_WH_GPIO, BLDC_PWM_WL_GPIO
);
// Encoder instance (AS5048A SPI absolute magnetic encoder)
MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, AS5_CSN_PIN);
// Instantiate the commander (SimpleFOC serial command helper)
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
/* Setup */
void setup()
{
// Use monitoring with serial
Serial.begin(115200);
WiFi.mode(WIFI_STA);
// Initialize ESP-NOW
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
// Register the receive callback
esp_now_register_recv_cb(receive_cb);
// If you want internal SimpleFOC debug prints, uncomment this,
// but it can slow down loopFOC() noticeably.
// motor.useMonitoring(Serial);
// Configure GPIOs
pinMode(ENABLE_PIN, OUTPUT);
pinMode(BLINK_PIN, OUTPUT);
pinMode(DRV0_CSN_PIN, OUTPUT);
pinMode(DRV1_SCK_PIN, OUTPUT);
// Set TMC6200 drive configuration (medium strength)
// (Your board uses these lines to set configuration. Keep as-is.)
digitalWrite(DRV1_SCK_PIN, LOW);
digitalWrite(DRV0_CSN_PIN, HIGH);
// Toggle reset to clear pending errors
digitalWrite(ENABLE_PIN, LOW);
delay(250);
digitalWrite(ENABLE_PIN, HIGH);
// Start SPI bus using your chosen pins
// SPI.begin(SCK, MISO, MOSI, SS)
SPI.begin(AS5_CLK_PIN, AS5_MISO_PIN, AS5_MOSI_PIN, AS5_CSN_PIN);
// Initialize sensor hardware on that SPI bus
// This sets up the driver to read angle data from the AS5048A
sensor.init(&SPI);
// If your motor direction is reversed (e.g. target angle increases but motor moves opposite),
// you can flip the sensor direction:
// sensor.direction = Direction::CW;
// sensor.direction = Direction::CCW;
// Link the motor to the encoder sensor
motor.linkSensor(&sensor);
// Power supply voltage [V]
driver.voltage_power_supply = 24; // Change to your real supply voltage
driver.init();
// Link the motor and the driver
motor.linkDriver(&driver);
// Aligning voltage [V]
// START SMALL THEN INCREASE IF NEEDED.
// This is the voltage used during initial alignment routine
motor.voltage_sensor_align = 1;
// Index search velocity [rad/s]
motor.velocity_index_search = 3;
// Set motion control loop to be used
// angle mode allows holding at a location
motor.controller = MotionControlType::angle;
// Velocity PI controller parameters
// Default parameters are in defaults.h
motor.PID_velocity.P = 0.05f;
motor.PID_velocity.I = 1;
motor.PID_velocity.D = 0;
// Voltage limit [V]
// START SMALL THEN INCREASE IF NEEDED.
motor.voltage_limit = 3;
// Jerk control using voltage ramp
// Default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 100;
// Velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01f;
// Angle P controller
motor.P_angle.P = 20;
// Maximal velocity of the position control
motor.velocity_limit = 150;
// Initialize motor hardware (driver + internal state)
motor.init();
// Align sensor and start FOC
// For an absolute encoder like AS5048A, this will:
// - establish electrical angle reference
// - ensure the motor commutation matches the encoder readings
motor.initFOC();
// Add target command "T"
// Example: send "T 1.57" to target ~90 degrees (in radians)
command.add('T', doTarget, "target angle");
Serial.println(F("Motor ready (AS5048A encoder)."));
delay(1000);
}
/* Loop */
void loop()
{
unsigned long currentMillis = millis();
// Demo motion pattern
// Every 500ms, we change the target by "offset" radians.
// After 18 changes, we reverse direction.
if (currentMillis - previousMillis >= 500)
{
// Blink LED for heartbeat
digitalWrite(BLINK_PIN, blinkState);
blinkState = !blinkState;
previousMillis = currentMillis;
}
// Print encoder angle every 100ms
if (currentMillis - lastPrintMillis >= 100)
{
Serial.print("Encoder angle (rad): ");
Serial.println(sensor.getAngle());
lastPrintMillis = currentMillis;
}
// Main FOC algorithm function
// The faster you run this function the better
motor.loopFOC();
if (toggle_request) {
toggle_request = false;
pos_state = !pos_state;
target_angle = pos_state ? cmd_received : 0;
Serial.printf("Toggled target_angle -> %.3f rad\n", target_angle);
}
// Motion control function
// Velocity, position (angle), or voltage depending on motor.controller
motor.move(target_angle);
// User communication
// Allows serial command parsing (e.g., "T 3.14")
command.run();
}

volatile bool toggle_request = false;
volatile unsigned int cmd_received = 0;
bool pos_state = false; // false -> pos A, true -> pos B

// Callback that triggers when data is received
void receive_cb(const esp_now_recv_info_t *info, const uint8_t *data, int len)
{
// Optionally print the sender MAC (PolyCast5)
Serial.print("From ");
for (int i = 0; i < 6; i++) {
Serial.printf("%02X", info->src_addr[i]);
if (i < 5) Serial.print(":");
}
Serial.print(" | ");
// The received data will be in the form "PolyCast5_Command_Value: %u" for security purposes
// We need to extract the %u (unsigned int) command
#define MAX_BUF_SIZE 65 // Maximum number of bytes the data could be
char buf[MAX_BUF_SIZE]; // Create a buffer to store the data string
// If the received data length is less than the max buffer size, make len the new buffer size
size_t copy_len = len < MAX_BUF_SIZE - 1 ? len : MAX_BUF_SIZE - 1;
memcpy(buf, data, copy_len); // Copy len bytes of the data into the buffer
buf[copy_len] = '\0'; // Null-terminate the string
// Try to parse "PolyCast5_Command_Value: %u" out of the string
if (sscanf(buf, "PolyCast5_Command_Value: %u", &cmd_received) == 1) { // If success -> data is valid and the command is now stored in cmd_received
Serial.print("Command: ");
Serial.println(cmd_received); // The command received
}
else { // Data is not valid
Serial.print("Unexpected data: ");
Serial.println(buf);
}
if (cmd_received != 0) {
toggle_request = true; // Request a toggle once
}
}

#define MAX_BUF_SIZE 65 // Maximum number of bytes the data could be
char buf[MAX_BUF_SIZE]; // Create a buffer to store the data string
// If the received data length is less than the max buffer size, make len the new buffer size
size_t copy_len = len < MAX_BUF_SIZE - 1 ? len : MAX_BUF_SIZE - 1;
memcpy(buf, data, copy_len); // Copy len bytes of the data into the buffer
buf[copy_len] = '\0'; // Null-terminate the string
// Try to parse "PolyCast5_Command_Value: %u" out of the string
if (sscanf(buf, "PolyCast5_Command_Value: %u", &cmd_received) == 1) { // If success -> data is valid and the command is now stored in cmd_received
Serial.print("Command: ");
Serial.println(cmd_received); // The command received
}
else { // Data is not valid
Serial.print("Unexpected data: ");
Serial.println(buf);
}

if (cmd_received != 0) {
toggle_request = true; // Request a toggle once
}

if (currentMillis - previousMillis >= 500)
{
// Blink LED for heartbeat
digitalWrite(BLINK_PIN, blinkState);
blinkState = !blinkState;
previousMillis = currentMillis;
}
// Print encoder angle every 100ms
if (currentMillis - lastPrintMillis >= 100)
{
Serial.print("Encoder angle (rad): ");
Serial.println(sensor.getAngle());
lastPrintMillis = currentMillis;
}