mirror of
https://github.com/joshr120/PD-Stepper.git
synced 2025-09-26 22:49:12 +08:00
Initial Commit
This commit is contained in:
commit
ad4a9f502f
BIN
Hardware/Accessories/Blinds Wheel.stl
Normal file
BIN
Hardware/Accessories/Blinds Wheel.stl
Normal file
Binary file not shown.
BIN
Hardware/Accessories/Turn Table.stl
Normal file
BIN
Hardware/Accessories/Turn Table.stl
Normal file
Binary file not shown.
BIN
Hardware/Accessories/Wheel (8mm Bore).stl
Normal file
BIN
Hardware/Accessories/Wheel (8mm Bore).stl
Normal file
Binary file not shown.
BIN
Hardware/Cover.stl
Normal file
BIN
Hardware/Cover.stl
Normal file
Binary file not shown.
1713
Hardware/Heat Spreader.step
Normal file
1713
Hardware/Heat Spreader.step
Normal file
File diff suppressed because it is too large
Load Diff
3723
Hardware/Machined Housing.step
Normal file
3723
Hardware/Machined Housing.step
Normal file
File diff suppressed because it is too large
Load Diff
BIN
PCB/GEREBR's/V1.0/PD Stepper V1.0.zip
Normal file
BIN
PCB/GEREBR's/V1.0/PD Stepper V1.0.zip
Normal file
Binary file not shown.
92
README.md
Normal file
92
README.md
Normal file
@ -0,0 +1,92 @@
|
||||
# **PD Stepper** - USB PD Closed Loop Stepper Driver & Controller
|
||||
PD Stepper is a USB PD Powered Nema 17 stepper driver using the Silent Trinamic Driver TMC2209 controlled with a ESP32-S3 for IoT and other applications.
|
||||
|
||||
<p float="left">
|
||||
<img src="https://github.com/joshr120/USB-PD-Stepper-Driver/assets/120012174/ba7bc5b1-e88a-49f0-8844-0d211900b3ac" width="48%" />
|
||||
<img src="https://github.com/joshr120/USB-PD-Stepper-Driver/assets/120012174/39841e2b-00d1-4257-9e5e-37f5bc7c2a4f" width="48%" />
|
||||
</p>
|
||||
|
||||
<p float="left">
|
||||
<img src="https://github.com/joshr120/USB-PD-Stepper-Driver/assets/120012174/d076abd4-243e-4dc6-b90f-01bfe40eda26" width="51%" />
|
||||
<img src="https://github.com/joshr120/USB-PD-Stepper-Driver/assets/120012174/720177eb-e481-40ec-9c04-c6366b8985e7" width="37%" />
|
||||
</p>
|
||||
|
||||
|
||||
Stepper motors are fantastic for projects, but assembling the necessary components—like a microcontroller, stepper driver, power supplies for both motor and microcontroller, and optionally an encoder can lead to bulkiness, high costs, complexity, and challenges in integrating them into compact or space-constrained designs. This project addresses these issues by consolidating everything onto a single board, eliminating the drawbacks with little to no compromises.
|
||||
|
||||
|
||||
The original inspiration for this project was to make a compact USB PD powered stepper driver for home automation of blinds. A whole lot of features were then added and this project now aims to create a versatile stepper motor driver and controller powered by USB Power Delivery (USB PD). By leveraging the capabilities of the ESP32-S3 microcontroller, along with the precision of the TMC2209 stepper motor driver and the sensing capabilities of the AS5600 magnetic rotary position sensor, this system provides a compact and flexible solution for a wide range of stepper motor applications.
|
||||
|
||||
## Main Features: ##
|
||||
- **TMC2209 Stepper Motor Driver:** Incorporates the TMC2209 stepper motor driver, renowned for its silent operation, high precision, and advanced features such as stealthChop™, spreadCycle™ and sensorless homing, ensuring smooth silent and efficient motor control.
|
||||
- **USB PD Power:** Utilizes the USB PD standard for power delivery, giving high power, a USB-C connector and no bulky 12 and 24V power bricks needed. It ensures compatibility with a variety of power sources and enabling seamless integration into existing setups. USB PD along with the TMC2209 can drive a stepper motor with upto ~50W of power.
|
||||
- **Form Factor:** Super compact forma factor, designed to fit on the back of a common Nema 17 stepper motor
|
||||
- **Onboard Rotary Position Sensor:** Integrates the AS5600 magnetic rotary position sensor for accurate and reliable absolute angle measurement, enabling precise positioning and closed loop control of the stepper motor.
|
||||
- **ESP32-S3 Control:** The ESP32-S3 microcontroller serves as the brains of the system, providing ample processing power, built-in Wi-Fi and Bluetooth connectivity, and a rich ecosystem of development tools and libraries for easy customization and expansion such as **ESPHome** and **ESPNow**.
|
||||
- Software or hardware selectable voltage: 5V, 9V, 12V, 15V or 20V all via USB PD
|
||||
- Auxiliary connectors and buttons for flexablity and expansion
|
||||
- Onboard 3.3V buck converter
|
||||
- Motor temperature measurment
|
||||
|
||||
## Current use cases & examples in the works: ##
|
||||
- Home automation with ESPHome and home assistant
|
||||
- Take in standard Step and Direction inputs and incorperate in closed loop control
|
||||
- Wireless position copying from one encoder to another motor
|
||||
- Camera slider with web interface for control and sensorlesshoming (sensorlesshoming example coming soon)
|
||||
|
||||
## PCB: ##
|
||||
PCB is a 4 layer 1.6mm custom PCB, this is what allows it to have its comapact form factor. The bare prototype boards were kindly provided by [PCBWay](https://pcbway.com/g/heg1oh). Schematic and GERBERs can be found in the PCB folder.
|
||||
|
||||
## Housing: ##
|
||||
The top cover can be 3D printed however this may not handle well if the motor & controller get warm under high load. A cover machined from polycarbonite or acrylic is a much better option (and looks awesome). An aluminium housing gives a sleek industrial look as well as acting as a heatsink, however this should not be used if you are using WiFi on the ESP32-S3 as it may block the antenna and reduce the WiFi signal.
|
||||

|
||||
|
||||
## Cooling: ##
|
||||
Stepper motor drivers can get warm under full load. The IC is cooled from the rear through a machined aluminium heatspreader which also acts as a spacer between the PCB and the motor. Two sizes of adhesive heatsinks can also stick to the top side with the larger one protruding through the housing and the smaller one siting just below the surface allowing the stepper to sit flat.
|
||||
|
||||
## Software: ##
|
||||
The software is currently still improving but current sample code should be more than enough to get you started
|
||||
|
||||
### ESPHome ###
|
||||
The provided .yaml file allows you to control the motor through ESPHOME (setup as blinds in this example). It allows you to use the buttons as inputs and set the USB PD voltage. It also outputs the AS5600 encoder reading to be used within ESPHome. The USB PD voltage and stepper microsteps can be set at startup.
|
||||
|
||||
### Arduino Code ###
|
||||
A few arduino examples are provided in this repo:
|
||||
- **Simple button control:**
|
||||
|
||||
This example uses the buttons on the side to run the motor as at a variety of speeds as well as requesting a specified voltage from the PD controller. This can be used for a simple turntable and uses the simple step and direction interface to the driver
|
||||
|
||||
- **Slider Webpage Control:**
|
||||
|
||||
In this example the ESP32 hosts a webpage which allows you to wirelessly control the stepper motors speed with a slider. This utilises the [TMC2209 library](https://github.com/janelia-arduino/TMC2209) to configure the stepper driver.
|
||||
|
||||
- **ESP-NOW Sender and Receiver:**
|
||||
|
||||
In this example one PD stepper board wirelessly sends its encoder value to another board which will mimic its position. This is an example of using ESP-NOW for low latency wireless communication.
|
||||
|
||||
More info on the software can be found on the [software page](Software/README.md).
|
||||
|
||||
## Control ##
|
||||
The primary way to control the PD Stepper is over WiFi (e.g ESPHome, ESP NOW or through a webserver) however there are many other ways you can control it
|
||||
|
||||
- Thanks to the pin mux on the ESP32-S3 the AUX connector can be used to send/receive commands over many different protocols. For example: Serial UART, I2C and standard STEP & DIR signals.
|
||||
- By Using the I2C connector as well as the AUX connector you could daisy chain many PD Stepper boards together.
|
||||
- The standard Qwiic / Stemma QT I2C connector means you can connect a wide range of sensors which could be used for control (for example a [Rotary Encoder](https://www.adafruit.com/product/5880) or a [Light Sensor](https://www.adafruit.com/product/4162))
|
||||
|
||||
- The 3 buttons on the side of the board can be set to control speed and/or position
|
||||
|
||||
- Serial commands can be sent/received through the USB-C connector (Note if your device sending the commands cannot provide enough power you may need an injector of some kind)
|
||||
|
||||
- The ESP32-S3 also has built in BLE (Bluetooth Low Energy) which could be used for control.
|
||||
|
||||
## Setting the driver voltage ##
|
||||
The USB PD voltage can be set by toggling 3 GPIO pins on the ESP32. This can be configured on the fly to change voltage.
|
||||
|
||||
By default the board will ask for 20V at startup if the microcontroller does not set the config pins fast enough. If for whatever reason your motor cannot handle 20V you may want to set resistor R18 to a set value or ensure the pins are set on startup. The voltage can be configed by software on the fly as required.
|
||||
|
||||
When programming the board it is likely your computer will only be able to provide 5V, the "Power Good" line from the PD controller can tell the ESP32 it is not receiving the voltage it is asking for and in this case you should proably not enable the stepper driver (as done in the example code)
|
||||
|
||||
NOTE: You must use a USB PD power brick/power bank which can provide the required voltage and current along with a suitable USB-C cable. QC (Quick Charge) is not currently supported.
|
||||
|
||||
## Setting the driver Current ##
|
||||
The motor current can be set either in hardware with the onbaord potentiometer or over the TMC2209 serial interface (see the [TMC2209 library](https://github.com/janelia-arduino/TMC2209) for for info on serial comms)
|
227
Software/Basic_Functionality_Test/Basic_Functionality_Test.ino
Normal file
227
Software/Basic_Functionality_Test/Basic_Functionality_Test.ino
Normal file
@ -0,0 +1,227 @@
|
||||
|
||||
/* Code to test the basic functionality of the USB PD Stepper Driver and Controller
|
||||
*
|
||||
* by Things by Josh 2024
|
||||
*/
|
||||
|
||||
|
||||
#include <Wire.h> //For I2C for encoder
|
||||
|
||||
/////////////////////////
|
||||
// Pin Defines //
|
||||
/////////////////////////
|
||||
|
||||
//TMC2209 Stepper Driver
|
||||
#define TMC_EN 21
|
||||
#define STEP 5
|
||||
#define DIR 6
|
||||
#define MS1 1
|
||||
#define MS2 2
|
||||
#define SPREAD 7
|
||||
#define TMC_TX 17
|
||||
#define TMC_RX 18
|
||||
#define DIAG 20
|
||||
#define INDEX 11
|
||||
|
||||
//PD Trigger (CH224K)
|
||||
#define PG 15 //power good singnal (dont enable stepper untill this is good)
|
||||
#define CFG1 38
|
||||
#define CFG2 48
|
||||
#define CFG3 47
|
||||
|
||||
//AS5600 Hall Effect Encoder
|
||||
#define SCL 9
|
||||
#define SDA 8
|
||||
|
||||
//Other
|
||||
#define VBUS 4
|
||||
#define NTC 7
|
||||
#define LED1 10
|
||||
#define LED2 12
|
||||
#define SW1 35
|
||||
#define SW2 36
|
||||
#define SW3 37
|
||||
#define AUX1 14
|
||||
#define AUX2 13
|
||||
|
||||
////////////////////////////
|
||||
// Global Variable Defines //
|
||||
////////////////////////////
|
||||
//TMC2209 Stepper Driver
|
||||
unsigned long lastStep = 0; //time of last step
|
||||
int stepDelay = 100; //delay (microseconds) between steps (AKA Speed)
|
||||
int motorDir = 0; //Stepper motor direction
|
||||
|
||||
//PD Trigger (CH224K)
|
||||
bool PGState = 0;
|
||||
|
||||
//AS5600 Hall Effect Encoder
|
||||
#define AS5600_ADDRESS 0x36 // I2C address of the AS5600 sensor
|
||||
signed long total_encoder_counts = 0;
|
||||
|
||||
//Other
|
||||
float VBusVoltage = 0;
|
||||
float VREF = 3.3;
|
||||
const float DIV_RATIO = 0.1189427313; //20k&2.7K Voltage Divider
|
||||
|
||||
//LED Flash
|
||||
long lastFlash = 0;
|
||||
int flashInt = 100;
|
||||
bool flashState = 0;
|
||||
|
||||
//Step state
|
||||
bool lastState = 0;
|
||||
|
||||
void setup() {
|
||||
|
||||
//TMC2209 Stepper Driver Setup (Simple not using serial comms)
|
||||
pinMode(TMC_EN, OUTPUT);
|
||||
pinMode(STEP, OUTPUT);
|
||||
pinMode(DIR, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
// pinMode(SPREAD, OUTPUT);
|
||||
// pinMode(INDEX, INPUT);
|
||||
// pinMode(DIAG, INPUT); //will need to be attached to interrupt for sensorless homing
|
||||
|
||||
digitalWrite(TMC_EN, HIGH); //High to disable on startup
|
||||
digitalWrite(MS1, LOW); //Microstep resolution configuration (internal pull-down resistors: MS2, MS1: 00: 1/8, 01: 1/32, 10: 1/64 11: 1/16
|
||||
digitalWrite(MS2, LOW);
|
||||
digitalWrite(DIR, motorDir); //set direction at start
|
||||
// Serial1.begin(115200, SERIAL_8N1, TMC_RX, TMC_TX); //will probably use a library in the future instead
|
||||
|
||||
|
||||
//PD Trigger Setup
|
||||
pinMode(PG, INPUT);
|
||||
pinMode(CFG1, OUTPUT);
|
||||
pinMode(CFG2, OUTPUT);
|
||||
pinMode(CFG3, OUTPUT);
|
||||
// 5V 9V 12V 15V 20V (Can also be changed on the fly)
|
||||
digitalWrite(CFG1, LOW); // 1 0 0 0 0
|
||||
digitalWrite(CFG2, LOW); // - 0 0 1 1
|
||||
digitalWrite(CFG3, HIGH); // - 0 1 1 0
|
||||
|
||||
|
||||
//General/Other Setup
|
||||
pinMode(LED1, OUTPUT);
|
||||
pinMode(LED2, OUTPUT);
|
||||
pinMode(SW1, INPUT);
|
||||
pinMode(SW2, INPUT);
|
||||
pinMode(SW3, INPUT);
|
||||
// pinMode(AUX1, INPUT); //can be set to anything
|
||||
// pinMode(AUX2, INPUT); //can be set to anything
|
||||
pinMode(NTC, INPUT);
|
||||
pinMode(VBUS, INPUT);
|
||||
|
||||
digitalWrite(LED1, LOW);
|
||||
digitalWrite(LED2, LOW);
|
||||
|
||||
|
||||
///////////////////////////////////////
|
||||
// Can't auto enter bootloader mode if
|
||||
// serial.begin has been called
|
||||
// so hold down SW2 on boot to enable serial
|
||||
// if you need to read outputs but not program
|
||||
///////////////////////////////////////
|
||||
//(Can also manually enter bootloader mode by holding BOOT, press RST, release BOOT)
|
||||
|
||||
if (digitalRead(SW2) == LOW){ //push = LOW
|
||||
Serial.begin(115200);
|
||||
Serial.println("Code Starting");
|
||||
}
|
||||
|
||||
//AS5600 Hall Encoder Setup
|
||||
Wire.begin(SDA, SCL); //start wire with earlier defined pins
|
||||
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
readVoltage(); //read and output voltage as well as PG status from PD trigger
|
||||
runMotor(); //run the stepper motor at set speed and direction (ensure readVoltage() called first to get PGState)
|
||||
readEncoder();
|
||||
|
||||
if (millis() - lastFlash > flashInt){
|
||||
lastFlash = millis();
|
||||
digitalWrite(LED1, flashState);
|
||||
if (flashState == 0){
|
||||
flashState = 1;
|
||||
} else {
|
||||
flashState = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
//run the stepper motor at set speed and direction (ensure readVoltage() called first to get PGState)
|
||||
void runMotor(){
|
||||
PGState = digitalRead(PG); //Low = power good!
|
||||
if (PGState == 0){ //only enable motor if correct voltage seen (to stop motor running off laptop 5V when programming)
|
||||
digitalWrite(TMC_EN, LOW); //low to enable (should really only do this on state change)
|
||||
digitalWrite(DIR, digitalRead(SW3)); //low to enable (should really only do this on state change)
|
||||
if (micros() - lastStep > stepDelay){
|
||||
lastStep = micros();
|
||||
digitalWrite(STEP, lastState);
|
||||
if (lastState == 0){
|
||||
lastState = 1;
|
||||
} else {
|
||||
lastState = 0;
|
||||
}
|
||||
}
|
||||
} else{
|
||||
digitalWrite(TMC_EN, HIGH); //disable stepper as voltage not as expected (should really only do this on state change)
|
||||
}
|
||||
}
|
||||
|
||||
//read and output voltage as well as PG status from PD trigger
|
||||
void readVoltage(){
|
||||
int ADCValue = analogRead(VBUS);
|
||||
VBusVoltage = ADCValue * (VREF / 4096.0) / DIV_RATIO;
|
||||
PGState = digitalRead(PG); //Low = power good!
|
||||
|
||||
|
||||
Serial.print("Voltage Read: ");
|
||||
Serial.print(VBusVoltage);
|
||||
Serial.println("V");
|
||||
|
||||
Serial.print("PG Status: ");
|
||||
Serial.println(PGState);
|
||||
}
|
||||
|
||||
//read the AS5600 encoder via I2C
|
||||
void readEncoder(){
|
||||
int raw_counts;
|
||||
static int prev_raw_counts = 0;
|
||||
static signed long revolutions = 0;
|
||||
float angle;
|
||||
// Request the raw encoder counts from the AS5600 sensor
|
||||
Wire.beginTransmission(AS5600_ADDRESS);
|
||||
Wire.write(0x0C); // Register address for raw angle output (0x0C)
|
||||
Wire.endTransmission(false);
|
||||
Wire.requestFrom(AS5600_ADDRESS, 2); // Request 2 bytes of data
|
||||
if (Wire.available() >= 2) {
|
||||
raw_counts = Wire.read() << 8 | Wire.read(); // Combine two bytes to get the counts value
|
||||
}
|
||||
|
||||
// Convert raw counts to angle in degrees if that is what's needed
|
||||
angle = (raw_counts * 360.0) / 4096.0;
|
||||
|
||||
// Check to see if it has gone past "home" over one full rotation
|
||||
if (prev_raw_counts > 3000 && raw_counts < 1000) {
|
||||
revolutions++;
|
||||
} else if (prev_raw_counts < 1000 && raw_counts > 3000) {
|
||||
revolutions--;
|
||||
}
|
||||
|
||||
// Update the previous raw counts
|
||||
prev_raw_counts = raw_counts;
|
||||
|
||||
total_encoder_counts = raw_counts + (raw_counts * revolutions);
|
||||
|
||||
|
||||
Serial.println("");
|
||||
Serial.print("Encoder Counts: ");
|
||||
Serial.println(total_encoder_counts);
|
||||
Serial.println("");
|
||||
}
|
@ -0,0 +1,168 @@
|
||||
/*
|
||||
ESP-NOW code from: https://RandomNerdTutorials.com/esp-now-esp32-arduino-ide/
|
||||
*/
|
||||
|
||||
#include <esp_now.h>
|
||||
#include <WiFi.h>
|
||||
|
||||
// Structure example to receive data
|
||||
// Must match the sender structure
|
||||
typedef struct struct_message {
|
||||
signed long pos;
|
||||
} struct_message;
|
||||
|
||||
// Create a struct_message called myData
|
||||
struct_message myData;
|
||||
|
||||
/////////////////////////
|
||||
// Pin Defines //
|
||||
/////////////////////////
|
||||
|
||||
//TMC2209 Stepper Driver
|
||||
#define TMC_EN 21 //Low to enable motor, high to disable)
|
||||
#define STEP 5
|
||||
#define DIR 6
|
||||
#define MS1 1
|
||||
#define MS2 2
|
||||
#define SPREAD 7
|
||||
#define TMC_TX 17
|
||||
#define TMC_RX 18
|
||||
#define DIAG 20
|
||||
#define INDEX 11
|
||||
|
||||
//PD Trigger (CH224K)
|
||||
#define PG 15 //power good singnal (dont enable stepper untill this is good)
|
||||
#define CFG1 38
|
||||
#define CFG2 48
|
||||
#define CFG3 47
|
||||
|
||||
//Other
|
||||
#define VBUS 4
|
||||
#define NTC 7
|
||||
#define LED1 10
|
||||
#define LED2 12
|
||||
#define SW1 35
|
||||
#define SW2 36
|
||||
#define SW3 37
|
||||
#define AUX1 14
|
||||
#define AUX2 13
|
||||
|
||||
////////////////////////////
|
||||
// Global Variable Defines //
|
||||
////////////////////////////
|
||||
//TMC2209 Stepper Driver
|
||||
unsigned long lastStep = 0; //time of last step
|
||||
int motorDir = 0; //Stepper motor direction
|
||||
bool lastState = 0;
|
||||
|
||||
//USB PD Trigger
|
||||
bool PGState = 0; //power good singal from trigger IC
|
||||
|
||||
//Other
|
||||
signed long setPos = 0; //pos received from sender
|
||||
int minDelay = 30; //max speed motor will turn at to reach setpoint
|
||||
signed long currentPos = 0; //current position (not using encoder yet)
|
||||
|
||||
// callback function that will be executed when data is received
|
||||
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
|
||||
memcpy(&myData, incomingData, sizeof(myData));
|
||||
// Serial.print("Position received: ");
|
||||
// Serial.println(myData.pos);
|
||||
setPos = myData.pos;
|
||||
}
|
||||
|
||||
void setup() {
|
||||
pinMode(SW1, INPUT);
|
||||
pinMode(SW2, INPUT);
|
||||
pinMode(SW3, INPUT);
|
||||
pinMode(LED1, OUTPUT);
|
||||
pinMode(LED2, OUTPUT);
|
||||
|
||||
//PD Trigger Setup
|
||||
pinMode(PG, INPUT);
|
||||
pinMode(CFG1, OUTPUT);
|
||||
pinMode(CFG2, OUTPUT);
|
||||
pinMode(CFG3, OUTPUT);
|
||||
// 5V 9V 12V 15V 20V (Can also be changed on the fly)
|
||||
digitalWrite(CFG1, LOW); // 1 0 0 0 0
|
||||
digitalWrite(CFG2, LOW); // - 0 0 1 1
|
||||
digitalWrite(CFG3, HIGH); // - 0 1 1 0
|
||||
|
||||
//Stepper simple setup (no serial comms)
|
||||
pinMode(TMC_EN, OUTPUT);
|
||||
pinMode(STEP, OUTPUT);
|
||||
pinMode(DIR, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
|
||||
digitalWrite(TMC_EN, HIGH); //High to disable on startup
|
||||
digitalWrite(MS1, HIGH); //Microstep resolution configuration (internal pull-down resistors: MS2, MS1: 00: 1/8, 01: 1/32, 10: 1/64 11: 1/16
|
||||
digitalWrite(MS2, LOW);
|
||||
|
||||
digitalWrite(LED1, LOW);
|
||||
digitalWrite(LED2, LOW);
|
||||
|
||||
|
||||
///////////////////////////////////////
|
||||
// Can't auto enter bootloader if
|
||||
// serial.begin has been called
|
||||
// so hold down SW2 on boot to enable serial
|
||||
// (wont auto enter bootloader mode)
|
||||
///////////////////////////////////////
|
||||
//(Can also manually enter bootloader mode by holding BOOT, press RST, release BOOT)
|
||||
|
||||
if (digitalRead(SW2) == LOW){ //push = LOW
|
||||
Serial.begin(115200);
|
||||
}
|
||||
|
||||
// Set device as a Wi-Fi Station
|
||||
WiFi.mode(WIFI_STA);
|
||||
|
||||
// Init ESP-NOW
|
||||
if (esp_now_init() != ESP_OK) {
|
||||
Serial.println("Error initializing ESP-NOW");
|
||||
return;
|
||||
}
|
||||
|
||||
// Once ESPNow is successfully Init, we will register for recv CB to
|
||||
// get recv packer info
|
||||
esp_now_register_recv_cb(OnDataRecv);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
noInterrupts();
|
||||
signed long setPoint = setPos*3.125;
|
||||
interrupts();
|
||||
signed long Error = currentPos - setPoint;
|
||||
int minDelay = 30;
|
||||
|
||||
if (abs(Error) < (550-minDelay)){ //200 is essentially the "Kp" gain
|
||||
minDelay = 550 - abs(Error);
|
||||
}
|
||||
|
||||
PGState = digitalRead(PG);
|
||||
if (currentPos != setPoint and PGState == 0){ //move motor
|
||||
// if (micros() - lastStep > minDelay){ //min time between steps (or use delay instead)
|
||||
digitalWrite(TMC_EN, LOW); //low to enable
|
||||
if (currentPos < setPoint){
|
||||
digitalWrite(DIR, HIGH);
|
||||
currentPos++;
|
||||
} else {
|
||||
digitalWrite(DIR, LOW);
|
||||
currentPos--;
|
||||
}
|
||||
lastStep = micros();
|
||||
digitalWrite(STEP, lastState);
|
||||
if (lastState == 0){
|
||||
lastState = 1;
|
||||
} else {
|
||||
lastState = 0;
|
||||
}
|
||||
// }
|
||||
|
||||
} else{
|
||||
digitalWrite(TMC_EN, HIGH); //disable motor when setpoint reached
|
||||
}
|
||||
|
||||
delayMicroseconds(minDelay); //set min time between steps
|
||||
}
|
@ -0,0 +1,219 @@
|
||||
/*
|
||||
ESP-NOW code from: https://RandomNerdTutorials.com/esp-now-esp32-arduino-ide/
|
||||
*/
|
||||
|
||||
#include <esp_now.h>
|
||||
#include <WiFi.h>
|
||||
|
||||
#include <Wire.h> //For I2C for encoder
|
||||
|
||||
// REPLACE WITH YOUR RECEIVER MAC Address
|
||||
uint8_t broadcastAddress[] = {0x63, 0xE8, 0x33, 0x60, 0x74, 0x5C}; //REPLACE WITH YOUR SENDER
|
||||
|
||||
// Structure example to send data
|
||||
// Must match the receiver structure
|
||||
typedef struct struct_message {
|
||||
signed long pos;
|
||||
} struct_message;
|
||||
|
||||
// Create a struct_message called myData
|
||||
struct_message myData;
|
||||
|
||||
esp_now_peer_info_t peerInfo;
|
||||
|
||||
/////////////////////////
|
||||
// Pin Defines //
|
||||
/////////////////////////
|
||||
|
||||
//TMC2209 Stepper Driver
|
||||
#define TMC_EN 21 //Low to enable motor, high to disable)
|
||||
#define STEP 5
|
||||
#define DIR 6
|
||||
#define MS1 1
|
||||
#define MS2 2
|
||||
#define SPREAD 7
|
||||
#define TMC_TX 17
|
||||
#define TMC_RX 18
|
||||
#define DIAG 20
|
||||
#define INDEX 11
|
||||
|
||||
//PD Trigger (CH224K)
|
||||
#define PG 15 //power good singnal (dont enable stepper untill this is good)
|
||||
#define CFG1 38
|
||||
#define CFG2 48
|
||||
#define CFG3 47
|
||||
|
||||
//Other
|
||||
#define VBUS 4
|
||||
#define NTC 7
|
||||
#define LED1 10
|
||||
#define LED2 12
|
||||
#define SW1 35
|
||||
#define SW2 36
|
||||
#define SW3 37
|
||||
#define AUX1 14
|
||||
#define AUX2 13
|
||||
|
||||
////////////////////////////
|
||||
// Global Variable Defines //
|
||||
////////////////////////////
|
||||
//USB PD Trigger
|
||||
bool PGState = 0; //power good singal from trigger IC
|
||||
|
||||
long lastFlash = 0;
|
||||
int flashInt = 100;
|
||||
bool flashState = 0;
|
||||
|
||||
//position to send
|
||||
signed long pos = 0;
|
||||
|
||||
//AS5600 Hall Effect Encoder
|
||||
#define AS5600_ADDRESS 0x36 // I2C address of the AS5600 sensor
|
||||
signed long total_encoder_counts = 0;
|
||||
|
||||
// callback when data is sent
|
||||
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
|
||||
// Serial.print("\r\nLast Packet Send Status:\t");
|
||||
// Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
|
||||
}
|
||||
|
||||
void setup() {
|
||||
pinMode(SW1, INPUT);
|
||||
pinMode(SW2, INPUT);
|
||||
pinMode(SW3, INPUT);
|
||||
pinMode(LED1, OUTPUT);
|
||||
pinMode(LED2, OUTPUT);
|
||||
|
||||
//PD Trigger Setup
|
||||
pinMode(PG, INPUT);
|
||||
pinMode(CFG1, OUTPUT);
|
||||
pinMode(CFG2, OUTPUT);
|
||||
pinMode(CFG3, OUTPUT);
|
||||
// 5V 9V 12V 15V 20V (Can also be changed on the fly)
|
||||
digitalWrite(CFG1, LOW); // 1 0 0 0 0
|
||||
digitalWrite(CFG2, LOW); // - 0 0 1 1
|
||||
digitalWrite(CFG3, HIGH); // - 0 1 1 0
|
||||
|
||||
digitalWrite(LED1, LOW);
|
||||
digitalWrite(LED2, LOW);
|
||||
|
||||
|
||||
///////////////////////////////////////
|
||||
// Can't auto enter bootloader if
|
||||
// serial.begin has been called
|
||||
// so hold down SW2 on boot to enable serial
|
||||
// (wont auto enter bootloader mode)
|
||||
///////////////////////////////////////
|
||||
//(Can also manually enter bootloader mode by holding BOOT, press RST, release BOOT)
|
||||
|
||||
if (digitalRead(SW2) == LOW){ //push = LOW
|
||||
Serial.begin(115200);
|
||||
}
|
||||
|
||||
|
||||
// Set device as a Wi-Fi Station
|
||||
WiFi.mode(WIFI_STA);
|
||||
|
||||
// Init ESP-NOW
|
||||
if (esp_now_init() != ESP_OK) {
|
||||
Serial.println("Error initializing ESP-NOW");
|
||||
return;
|
||||
}
|
||||
|
||||
// Once ESPNow is successfully Init, we will register for Send CB to
|
||||
// get the status of Trasnmitted packet
|
||||
esp_now_register_send_cb(OnDataSent);
|
||||
|
||||
// Register peer
|
||||
memcpy(peerInfo.peer_addr, broadcastAddress, 6);
|
||||
peerInfo.channel = 0;
|
||||
peerInfo.encrypt = false;
|
||||
|
||||
// Add peer
|
||||
if (esp_now_add_peer(&peerInfo) != ESP_OK){
|
||||
Serial.println("Failed to add peer");
|
||||
return;
|
||||
}
|
||||
|
||||
//AS5600 Hall Encoder Setup
|
||||
Wire.begin(SDA, SCL); //start wire with earlier defined pins
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//flash LED to show code is running
|
||||
if (millis() - lastFlash > flashInt){
|
||||
lastFlash = millis();
|
||||
digitalWrite(LED1, flashState);
|
||||
if (flashState == 0){
|
||||
flashState = 1;
|
||||
} else {
|
||||
flashState = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int raw_counts;
|
||||
static int prev_raw_counts = 0;
|
||||
static signed long revolutions = 0;
|
||||
float angle;
|
||||
// Request the raw encoder counts from the AS5600 sensor
|
||||
Wire.beginTransmission(AS5600_ADDRESS);
|
||||
Wire.write(0x0C); // Register address for raw angle output (0x0C)
|
||||
Wire.endTransmission(false);
|
||||
Wire.requestFrom(AS5600_ADDRESS, 2); // Request 2 bytes of data
|
||||
if (Wire.available() >= 2) {
|
||||
raw_counts = Wire.read() << 8 | Wire.read(); // Combine two bytes to get the counts value
|
||||
}
|
||||
|
||||
// Convert raw counts to angle in degrees if that is what's needed
|
||||
angle = (raw_counts * 360.0) / 4096.0;
|
||||
|
||||
// Check to see if it has gone past "home" over one full rotation
|
||||
if (prev_raw_counts > 3000 && raw_counts < 1000) {
|
||||
revolutions++;
|
||||
} else if (prev_raw_counts < 1000 && raw_counts > 3000) {
|
||||
revolutions--;
|
||||
}
|
||||
|
||||
// Update the previous raw counts
|
||||
prev_raw_counts = raw_counts;
|
||||
|
||||
total_encoder_counts = raw_counts + (4096 * revolutions);
|
||||
|
||||
|
||||
Serial.println("");
|
||||
Serial.print("Encoder Counts: ");
|
||||
Serial.println(total_encoder_counts);
|
||||
Serial.println("");
|
||||
|
||||
|
||||
// if (digitalRead(SW3) == LOW) {
|
||||
// pos = pos + 300;
|
||||
// Serial.print("Pos increased to: ");
|
||||
// Serial.println(pos);
|
||||
// }
|
||||
//
|
||||
// if (digitalRead(SW1) == LOW) {
|
||||
// pos = pos - 300;
|
||||
// Serial.print("pos decreased to: ");
|
||||
// Serial.println(pos);
|
||||
// }
|
||||
//
|
||||
// if (digitalRead(SW2) == LOW) {
|
||||
// pos = 0;
|
||||
// Serial.println("pos reset");
|
||||
// }
|
||||
|
||||
|
||||
// Set values to send
|
||||
// myData.pos = pos;
|
||||
myData.pos = total_encoder_counts;
|
||||
|
||||
// Send message via ESP-NOW (should probably schedule as doesnt need to be too fast
|
||||
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));
|
||||
|
||||
if (result != ESP_OK) {
|
||||
Serial.println("Error sending the data");
|
||||
}
|
||||
delay(20);
|
||||
}
|
186
Software/ESPHome/PD-Stepper.yaml
Normal file
186
Software/ESPHome/PD-Stepper.yaml
Normal file
@ -0,0 +1,186 @@
|
||||
# PD Stepper ESPHome Config
|
||||
# https://github.com/joshr120/PD-Stepper
|
||||
|
||||
# Todo:
|
||||
# - add sensorless homing/serial comms to TMC2209
|
||||
# - VBUS_SENSE adc read
|
||||
# - Motor NTC read
|
||||
|
||||
|
||||
# Enable Home Assistant API
|
||||
api:
|
||||
encryption:
|
||||
key: "u1qvnGLD6JjRQGNodaYXFFXSPxe7N+3wSKcwurJPhCM="
|
||||
|
||||
ota:
|
||||
password: "07b9f5a01b7f15d6523941ddf8f4b3fc"
|
||||
|
||||
wifi:
|
||||
ssid: !secret wifi_ssid
|
||||
password: !secret wifi_password
|
||||
|
||||
# Enable fallback hotspot (captive portal) in case wifi connection fails
|
||||
ap:
|
||||
ssid: "PD-Stepper Fallback Hotspot"
|
||||
password: "password"
|
||||
|
||||
captive_portal:
|
||||
|
||||
|
||||
esphome:
|
||||
name: stepper-motor
|
||||
# Set microstepping and PD voltage pins on boot
|
||||
on_boot:
|
||||
priority: 600
|
||||
then:
|
||||
- output.turn_off: #Microstep resolution configuration (internal pull-down resistors: MS2, MS1: 00: 1/8, 01: 1/32, 10: 1/64 11: 1/16
|
||||
id: MS1_pin
|
||||
- output.turn_off:
|
||||
id: MS2_pin
|
||||
# 5V 9V 12V 15V 20V
|
||||
- output.turn_off: # 1 0 0 0 0
|
||||
id: CFG1_pin
|
||||
- output.turn_off: # - 0 0 1 1
|
||||
id: CFG2_pin
|
||||
- output.turn_on: # - 0 1 1 0
|
||||
id: CFG3_pin
|
||||
|
||||
esp32:
|
||||
board: esp32-s3-devkitc-1
|
||||
|
||||
# Enable logging
|
||||
logger:
|
||||
|
||||
# Define GPIO pins for microstepping & PD voltage configuration
|
||||
output:
|
||||
- platform: gpio
|
||||
pin: GPIO1
|
||||
id: MS1_pin
|
||||
- platform: gpio
|
||||
pin: GPIO2
|
||||
id: MS2_pin
|
||||
|
||||
- platform: gpio
|
||||
pin: GPIO38
|
||||
id: CFG1_pin
|
||||
- platform: gpio
|
||||
pin: GPIO48
|
||||
id: CFG2_pin
|
||||
- platform: gpio
|
||||
pin: GPIO47
|
||||
id: CFG3_pin
|
||||
|
||||
|
||||
# Define binary input sensor for power_good and push button
|
||||
binary_sensor:
|
||||
- platform: gpio
|
||||
pin:
|
||||
number: GPIO15 # PG Pin
|
||||
mode: INPUT
|
||||
inverted: true
|
||||
name: "Power Good"
|
||||
device_class: power
|
||||
filters: #Debounce
|
||||
- delayed_on: 10ms
|
||||
|
||||
- platform: gpio
|
||||
pin:
|
||||
number: GPIO35 # Button 1 Pin
|
||||
mode: INPUT
|
||||
name: "Button 1"
|
||||
filters: #Debounce
|
||||
- delayed_on: 10ms
|
||||
|
||||
- platform: gpio
|
||||
pin:
|
||||
number: GPIO36 # Button 2 Pin
|
||||
mode: INPUT
|
||||
name: "Button 2"
|
||||
filters: #Debounce
|
||||
- delayed_on: 10ms
|
||||
|
||||
- platform: gpio
|
||||
pin:
|
||||
number: GPIO37 # Button 3 Pin
|
||||
mode: INPUT
|
||||
name: "Button 3"
|
||||
filters: #Debounce
|
||||
- delayed_on: 10ms
|
||||
|
||||
# Example configuration entry
|
||||
stepper:
|
||||
- platform: a4988 #using this for now as similar to TMC2209
|
||||
id: my_stepper
|
||||
step_pin: GPIO5
|
||||
dir_pin:
|
||||
number: GPIO6
|
||||
inverted: false #invert to change direction
|
||||
max_speed: 16000 steps/s #will depend on which microstep mode is set
|
||||
|
||||
sleep_pin:
|
||||
number: GPIO21
|
||||
inverted: true #true for trinamic
|
||||
acceleration: 600
|
||||
deceleration: 800
|
||||
|
||||
# Create cover (blinds) using stepper motor
|
||||
cover:
|
||||
- platform: template
|
||||
name: "Office Blinds"
|
||||
id: office_blinds
|
||||
device_class: blind
|
||||
open_action:
|
||||
- stepper.set_target:
|
||||
id: my_stepper
|
||||
target: 0
|
||||
- sensor.template.publish:
|
||||
id: position
|
||||
state: !lambda return id(my_stepper).target_position;
|
||||
close_action:
|
||||
- stepper.set_target:
|
||||
id: my_stepper
|
||||
target: 77000 #how far it will travel
|
||||
- sensor.template.publish:
|
||||
id: position
|
||||
state: !lambda return id(my_stepper).target_position;
|
||||
stop_action:
|
||||
- stepper.set_target:
|
||||
id: my_stepper
|
||||
target: !lambda return id(my_stepper).current_position;
|
||||
- sensor.template.publish:
|
||||
id: position
|
||||
state: !lambda return id(my_stepper).current_position;
|
||||
|
||||
optimistic: true
|
||||
assumed_state: true
|
||||
|
||||
|
||||
#AS5600 rotary encoder
|
||||
i2c:
|
||||
sda: GPIO8
|
||||
scl: GPIO9
|
||||
scan: true
|
||||
id: bus_a
|
||||
|
||||
as5600:
|
||||
slow_filter: 16x #default 16x
|
||||
|
||||
|
||||
sensor:
|
||||
# Blinds
|
||||
- platform: template
|
||||
name: "Blinds 1 Position"
|
||||
id: position
|
||||
|
||||
#AS5600 Rotary Encoder
|
||||
- platform: as5600
|
||||
name: Position
|
||||
update_interval: 5s
|
||||
raw_position:
|
||||
name: Raw Position
|
||||
gain:
|
||||
name: Gain
|
||||
magnitude:
|
||||
name: Magnitude
|
||||
status:
|
||||
name: Status
|
23
Software/README.md
Normal file
23
Software/README.md
Normal file
@ -0,0 +1,23 @@
|
||||
# **PD Stepper** - More Software Info
|
||||
|
||||
## Arduino Upload settings: ##
|
||||
When uplooading software with the Arduino IDE ensure you have firtst installed the ESP32 Add-on in Arduino IDE there are [many tutorials](https://randomnerdtutorials.com/installing-esp32-arduino-ide-2-0/) on how to do this.
|
||||
|
||||
The Board type should be set as "ESP32S3 Dev Module"
|
||||
|
||||
USB CDC on Boot should be set to "Enabled"
|
||||
|
||||

|
||||
|
||||
## Arduino Serial Bug: ##
|
||||
There is currently a software bug being worked on which does not allow serial communication over USB to work at the same time as programming. A program may fail to upload if Serial.begin() has previously been called.
|
||||
|
||||
There are a couple of work arounds for this;
|
||||
- You can manaully enter bootloader mode by holding the BOOT button, pressing and releasing the RST button and then releasing the BOOT button.
|
||||
- Alternatively software can be set up such Serial.begin() is only called on startup if a button is held at boot. This will allow USB serial commication on this power cycle but not programming (This is done in all example sketches). Could also be implemented the opposite way if you plan on using serial comms more often than uploading.
|
||||
- This bug is not present when using ESPHome.
|
||||
|
||||
## ESPHome: ##
|
||||
The current ESPHome .yaml config file treats the TMC2209 as a a4988 driver as there is no TMC2209 intergration yet. The microsteps and PD voltage are set at startup by GPIO pins and the motor is driven via the STEP and DIR pins. A future intergration could allow for advanced TMC2209 features such as sensorless homing (Could also manually talk to the TMC2209 with the [UART Bus component](https://esphome.io/components/uart.html))
|
||||
|
||||
Other interfaces exposed to ESPHOME include the POWER GOOD signal, this is a signal indicating the PD Stepper is getting the requested voltage from the USB power supply. The 3 buttons on the side are also set up is binary sensors and the encoder position is read using the [AS5600 component](https://esphome.io/components/sensor/as5600.html).
|
204
Software/Simple_Button_Control/Simple_Button_Control.ino
Normal file
204
Software/Simple_Button_Control/Simple_Button_Control.ino
Normal file
@ -0,0 +1,204 @@
|
||||
|
||||
|
||||
/* Code to run a stpper motor with buttons using the USB PD Stepper Driver and Controller
|
||||
*
|
||||
* by Things by Josh 2024
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// Pin Defines //
|
||||
/////////////////////////
|
||||
|
||||
//TMC2209 Stepper Driver
|
||||
#define TMC_EN 21 //Low to enable motor, high to disable)
|
||||
#define STEP 5
|
||||
#define DIR 6
|
||||
#define MS1 1
|
||||
#define MS2 2
|
||||
#define SPREAD 7
|
||||
#define TMC_TX 17
|
||||
#define TMC_RX 18
|
||||
#define DIAG 20
|
||||
#define INDEX 11
|
||||
|
||||
//PD Trigger (CH224K)
|
||||
#define PG 15 //power good singnal (dont enable stepper untill this is good)
|
||||
#define CFG1 38
|
||||
#define CFG2 48
|
||||
#define CFG3 47
|
||||
|
||||
//Other
|
||||
#define VBUS 4
|
||||
#define NTC 7
|
||||
#define LED1 10
|
||||
#define LED2 12
|
||||
#define SW1 35
|
||||
#define SW2 36
|
||||
#define SW3 37
|
||||
#define AUX1 14
|
||||
#define AUX2 13
|
||||
|
||||
|
||||
////////////////////////////
|
||||
// Global Variable Defines //
|
||||
////////////////////////////
|
||||
//TMC2209 Stepper Driver
|
||||
unsigned long lastStep = 0; //time of last step
|
||||
int motorDir = 0; //Stepper motor direction
|
||||
bool lastState = 0;
|
||||
|
||||
//USB PD Trigger
|
||||
bool PGState = 0; //power good singal from trigger IC
|
||||
|
||||
//buttons and motor control
|
||||
int counter = 0;
|
||||
bool incButtonState = HIGH;
|
||||
bool decButtonState = HIGH;
|
||||
bool resetButtonState = HIGH;
|
||||
unsigned long lastDebounceTime = 0;
|
||||
unsigned long debounceDelay = 50;
|
||||
|
||||
int delays[] = {0, 2000, 1000, 700, 400, 200, 100, 75, 50, 40, 30}; //list of speeds (higher = slower, 0 = stop)
|
||||
int numSpeeds = 10;
|
||||
|
||||
long lastFlash = 0;
|
||||
int flashInt = 100;
|
||||
bool flashState = 0;
|
||||
|
||||
|
||||
|
||||
|
||||
void setup() {
|
||||
// put your setup code here, to run once:
|
||||
|
||||
pinMode(SW1, INPUT);
|
||||
pinMode(SW2, INPUT);
|
||||
pinMode(SW3, INPUT);
|
||||
pinMode(LED1, OUTPUT);
|
||||
pinMode(LED2, OUTPUT);
|
||||
|
||||
//PD Trigger Setup
|
||||
pinMode(PG, INPUT);
|
||||
pinMode(CFG1, OUTPUT);
|
||||
pinMode(CFG2, OUTPUT);
|
||||
pinMode(CFG3, OUTPUT);
|
||||
// 5V 9V 12V 15V 20V (Can also be changed on the fly)
|
||||
digitalWrite(CFG1, LOW); // 1 0 0 0 0
|
||||
digitalWrite(CFG2, LOW); // - 0 0 1 1
|
||||
digitalWrite(CFG3, HIGH); // - 0 1 1 0
|
||||
|
||||
//Stepper simple setup (no serial comms)
|
||||
pinMode(TMC_EN, OUTPUT);
|
||||
pinMode(STEP, OUTPUT);
|
||||
pinMode(DIR, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
|
||||
digitalWrite(TMC_EN, HIGH); //High to disable on startup
|
||||
digitalWrite(MS1, HIGH); //Microstep resolution configuration (internal pull-down resistors: MS2, MS1: 00: 1/8, 01: 1/32, 10: 1/64 11: 1/16
|
||||
digitalWrite(MS2, LOW);
|
||||
|
||||
digitalWrite(LED1, LOW);
|
||||
digitalWrite(LED2, LOW);
|
||||
|
||||
|
||||
///////////////////////////////////////
|
||||
// Can't auto enter bootloader if
|
||||
// serial.begin has been called
|
||||
// so hold down SW2 on boot to enable serial
|
||||
// (wont auto enter bootloader mode)
|
||||
///////////////////////////////////////
|
||||
//(Can also manually enter bootloader mode by holding BOOT, press RST, release BOOT)
|
||||
|
||||
if (digitalRead(SW2) == LOW){ //push = LOW
|
||||
Serial.begin(115200);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//flash LED to show code is running
|
||||
if (millis() - lastFlash > flashInt){
|
||||
lastFlash = millis();
|
||||
digitalWrite(LED1, flashState);
|
||||
if (flashState == 0){
|
||||
flashState = 1;
|
||||
} else {
|
||||
flashState = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Read and doubounce inputs and incriment counter
|
||||
if ((millis() - lastDebounceTime) > debounceDelay) {
|
||||
lastDebounceTime = millis();
|
||||
bool currentIncButtonState = digitalRead(SW3);
|
||||
bool currentDecButtonState = digitalRead(SW1);
|
||||
bool currentResetButtonState = digitalRead(SW2);
|
||||
|
||||
if (currentIncButtonState != incButtonState) {
|
||||
incButtonState = currentIncButtonState;
|
||||
if (incButtonState == LOW) {
|
||||
counter++;
|
||||
if (counter > numSpeeds){
|
||||
counter = numSpeeds;
|
||||
}
|
||||
Serial.print("Counter increased to: ");
|
||||
Serial.println(counter);
|
||||
}
|
||||
}
|
||||
|
||||
if (currentDecButtonState != decButtonState) {
|
||||
decButtonState = currentDecButtonState;
|
||||
if (decButtonState == LOW) {
|
||||
counter--;
|
||||
if (counter < (numSpeeds*-1)){
|
||||
counter = (numSpeeds*-1);
|
||||
}
|
||||
Serial.print("Counter decreased to: ");
|
||||
Serial.println(counter);
|
||||
}
|
||||
}
|
||||
|
||||
if (currentResetButtonState != resetButtonState) {
|
||||
resetButtonState = currentResetButtonState;
|
||||
if (resetButtonState == LOW) {
|
||||
counter = 0;
|
||||
Serial.println("Counter reset");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//read PG to check if motor shoulf be enabled (this should be scheduled to not run as often)
|
||||
PGState = digitalRead(PG);
|
||||
if (counter != 0 and PGState == 0){ //only enable motor if correct voltage seen (to stop motor running off laptop 5V when programming)
|
||||
digitalWrite(TMC_EN, LOW); //low to enable (should really only do this on state change)
|
||||
|
||||
int abs_counter = 0;
|
||||
if (counter > 0){ //set direction (should only do this on Dir change really)
|
||||
digitalWrite(DIR, HIGH);
|
||||
abs_counter = counter;
|
||||
} else {
|
||||
digitalWrite(DIR, LOW);
|
||||
abs_counter = counter*-1;
|
||||
}
|
||||
|
||||
if (micros() - lastStep > delays[abs_counter]){
|
||||
lastStep = micros();
|
||||
digitalWrite(STEP, lastState);
|
||||
if (lastState == 0){
|
||||
lastState = 1;
|
||||
} else {
|
||||
lastState = 0;
|
||||
}
|
||||
}
|
||||
} else{
|
||||
digitalWrite(TMC_EN, HIGH); //disable stepper as voltage not as expected (should really only do this on state change)
|
||||
}
|
||||
}
|
226
Software/Slider_Webpage_Control/Slider_Webpage_Control.ino
Normal file
226
Software/Slider_Webpage_Control/Slider_Webpage_Control.ino
Normal file
@ -0,0 +1,226 @@
|
||||
/*
|
||||
* USB PD Stepper Slider demo code
|
||||
* TODO:
|
||||
* - Stall gaurd for sensorless homing
|
||||
* - Disable interupt when reading from set_speed (better handling in general could set speed in handler?)
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <ESPAsyncWebServer.h>
|
||||
#include <AsyncTCP.h>
|
||||
|
||||
#include <TMC2209.h> // https://github.com/janelia-arduino/TMC2209/tree/main
|
||||
|
||||
const char *ssid = "Slider";
|
||||
const char *password = "password";
|
||||
|
||||
#include "index_html.h"
|
||||
|
||||
AsyncWebServer server(80);
|
||||
|
||||
//TMC2209 setup
|
||||
TMC2209 stepper_driver;
|
||||
HardwareSerial & serial_stream = Serial2;
|
||||
const long SERIAL_BAUD_RATE = 115200;
|
||||
|
||||
const uint8_t RUN_CURRENT_PERCENT = 100; //how much current to run at (0-100%)
|
||||
|
||||
//TMC2209 Stepper Driver
|
||||
#define TMC_EN 21
|
||||
#define STEP 5
|
||||
#define DIR 6
|
||||
#define MS1 1
|
||||
#define MS2 2
|
||||
#define SPREAD 7
|
||||
#define TMC_TX 17
|
||||
#define TMC_RX 18
|
||||
#define DIAG 20
|
||||
#define INDEX 11
|
||||
|
||||
//PD Trigger (CH224K)
|
||||
#define PG 15 //power good singnal (dont enable stepper untill this is good)
|
||||
#define CFG1 38
|
||||
#define CFG2 48
|
||||
#define CFG3 47
|
||||
|
||||
//Other
|
||||
#define VBUS 4
|
||||
#define NTC 7
|
||||
#define LED1 10
|
||||
#define LED2 12
|
||||
#define SW1 35
|
||||
#define SW2 36
|
||||
#define SW3 37
|
||||
#define AUX1 14
|
||||
#define AUX2 13
|
||||
|
||||
|
||||
//Global variables
|
||||
int set_speed = 0;
|
||||
bool PGState = 0; //state of the power good signal from PD sink IC
|
||||
|
||||
//flashing LED
|
||||
long lastFlash = 0;
|
||||
int flashInt = 100;
|
||||
bool flashState = 0;
|
||||
|
||||
//button read and debounce
|
||||
bool incButtonState = HIGH;
|
||||
bool decButtonState = HIGH;
|
||||
bool resetButtonState = HIGH;
|
||||
unsigned long lastDebounceTime = 0;
|
||||
unsigned long debounceDelay = 50;
|
||||
|
||||
void setup() {
|
||||
//PD Trigger Setup
|
||||
pinMode(PG, INPUT);
|
||||
pinMode(CFG1, OUTPUT);
|
||||
pinMode(CFG2, OUTPUT);
|
||||
pinMode(CFG3, OUTPUT);
|
||||
// 5V 9V 12V 15V 20V (Can also be changed on the fly)
|
||||
digitalWrite(CFG1, LOW); // 1 0 0 0 0
|
||||
digitalWrite(CFG2, LOW); // - 0 0 1 1
|
||||
digitalWrite(CFG3, HIGH); // - 0 1 1 0
|
||||
|
||||
//General
|
||||
pinMode(SW1, INPUT);
|
||||
pinMode(SW2, INPUT);
|
||||
pinMode(SW3, INPUT);
|
||||
pinMode(LED1, OUTPUT);
|
||||
pinMode(LED2, OUTPUT);
|
||||
|
||||
|
||||
//Setup serial comms with TMC2209
|
||||
pinMode(MS1, OUTPUT);
|
||||
pinMode(MS1, OUTPUT);
|
||||
pinMode(TMC_EN, OUTPUT); //maybe not needed as this done over serial
|
||||
digitalWrite(TMC_EN, LOW); //Enabled here and later enabled/disabled over UART
|
||||
|
||||
digitalWrite(MS1, LOW); //used to set serial address in UART mode
|
||||
digitalWrite(MS2, LOW);
|
||||
|
||||
stepper_driver.setup(serial_stream, SERIAL_BAUD_RATE, TMC2209::SERIAL_ADDRESS_0, TMC_RX, TMC_TX);
|
||||
stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT);
|
||||
stepper_driver.enableAutomaticCurrentScaling(); /////////TESTING
|
||||
// stepper_driver.enableCoolStep();
|
||||
stepper_driver.disable();
|
||||
|
||||
///////////////////////////////////////
|
||||
// Can't auto enter bootloader if
|
||||
// serial.begin has been called
|
||||
// so hold down SW2 on boot to enable serial
|
||||
// if you need to read outputs but not program
|
||||
///////////////////////////////////////
|
||||
//(Can also manually enter bootloader mode by holding BOOT, press RST, release BOOT)
|
||||
|
||||
if (digitalRead(SW2) == LOW){ //push = LOW
|
||||
Serial.begin(115200);
|
||||
Serial.println("Code Starting");
|
||||
}
|
||||
|
||||
// Set up ESP32 as an Access Point
|
||||
WiFi.softAP(ssid, password);
|
||||
IPAddress ip = WiFi.softAPIP();
|
||||
Serial.print("AP IP address: ");
|
||||
Serial.println(ip);
|
||||
|
||||
// Route to handle slider position update
|
||||
server.on("/update", HTTP_POST, [](AsyncWebServerRequest *request) {
|
||||
if (request->hasParam("slider", true)) {
|
||||
AsyncWebParameter* sliderParam = request->getParam("slider", true);
|
||||
String sliderValue = sliderParam->value();
|
||||
set_speed = sliderValue.toInt(); //convert to int
|
||||
Serial.println("Slider value: " + sliderValue);
|
||||
}
|
||||
request->send(200); // Respond with HTTP 200 OK
|
||||
});
|
||||
|
||||
// Serve HTML page with JavaScript for slider position update
|
||||
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) {
|
||||
request->send_P(200, "text/html", index_html);
|
||||
});
|
||||
|
||||
server.begin();
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//flash LED to show code is running
|
||||
if (millis() - lastFlash > flashInt){
|
||||
lastFlash = millis();
|
||||
digitalWrite(LED1, flashState);
|
||||
if (flashState == 0){
|
||||
flashState = 1;
|
||||
} else {
|
||||
flashState = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if ((millis() - lastDebounceTime) > debounceDelay) {
|
||||
lastDebounceTime = millis();
|
||||
bool currentIncButtonState = digitalRead(SW3);
|
||||
bool currentDecButtonState = digitalRead(SW1);
|
||||
bool currentResetButtonState = digitalRead(SW2);
|
||||
|
||||
if (currentIncButtonState != incButtonState) {
|
||||
incButtonState = currentIncButtonState;
|
||||
if (incButtonState == LOW) {
|
||||
set_speed = set_speed + 10;
|
||||
if (set_speed > 100){
|
||||
set_speed = 100;
|
||||
}
|
||||
Serial.print("Set speed increased to: ");
|
||||
Serial.println(set_speed);
|
||||
}
|
||||
}
|
||||
|
||||
if (currentDecButtonState != decButtonState) {
|
||||
decButtonState = currentDecButtonState;
|
||||
if (decButtonState == LOW) {
|
||||
set_speed = set_speed -10;
|
||||
if (set_speed < -100){
|
||||
set_speed = -100;
|
||||
}
|
||||
Serial.print("Set speed decreased to: ");
|
||||
Serial.println(set_speed);
|
||||
}
|
||||
}
|
||||
|
||||
if (currentResetButtonState != resetButtonState) {
|
||||
resetButtonState = currentResetButtonState;
|
||||
if (resetButtonState == LOW) {
|
||||
set_speed = 0;
|
||||
Serial.println("Counter reset");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
PGState = digitalRead(PG);
|
||||
|
||||
//should probably only enable/disable on state change
|
||||
if (PGState == 0){ //only enable motor if correct voltage seen (to stop motor running off laptop 5V when programming etc)
|
||||
stepper_driver.enable();
|
||||
int temp_speed = set_speed;
|
||||
Serial.println(temp_speed);
|
||||
if (temp_speed == 0){
|
||||
stepper_driver.moveAtVelocity(0);
|
||||
} else if (set_speed > 0){
|
||||
stepper_driver.enableInverseMotorDirection();
|
||||
stepper_driver.moveAtVelocity(temp_speed*600);
|
||||
} else{
|
||||
stepper_driver.disableInverseMotorDirection();
|
||||
stepper_driver.moveAtVelocity(temp_speed*-600);
|
||||
}
|
||||
}
|
||||
else {
|
||||
stepper_driver.disable();
|
||||
}
|
||||
|
||||
delay(30);
|
||||
|
||||
}
|
267
Software/Slider_Webpage_Control/index_html.h
Normal file
267
Software/Slider_Webpage_Control/index_html.h
Normal file
@ -0,0 +1,267 @@
|
||||
|
||||
const char index_html[] PROGMEM = R"rawliteral(
|
||||
<!DOCTYPE html>
|
||||
<html>
|
||||
<head>
|
||||
<title>Slider Position</title>
|
||||
<style>
|
||||
|
||||
body {
|
||||
background-color: #383838;
|
||||
color: white;
|
||||
font-family: Arial, Helvetica, sans-serif;
|
||||
}
|
||||
|
||||
.slider1 {
|
||||
width: 40%;
|
||||
height: 40px;
|
||||
}
|
||||
|
||||
|
||||
/* Style the toggle switch container */
|
||||
.toggle-container {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
align-items: center;
|
||||
margin-bottom: 20px; /* Adjust spacing between checkbox and slider */
|
||||
}
|
||||
|
||||
/* Style the toggle switch */
|
||||
.toggle-switch {
|
||||
position: relative;
|
||||
display: inline-block;
|
||||
width: 60px;
|
||||
height: 34px;
|
||||
}
|
||||
|
||||
/* Hide the default checkbox */
|
||||
.toggle-switch input {
|
||||
opacity: 0;
|
||||
width: 0;
|
||||
height: 0;
|
||||
}
|
||||
|
||||
/* The slider */
|
||||
.slider {
|
||||
position: absolute;
|
||||
cursor: pointer;
|
||||
top: 0;
|
||||
left: 0;
|
||||
right: 0;
|
||||
bottom: 0;
|
||||
background-color: #ccc;
|
||||
-webkit-transition: .4s;
|
||||
transition: .4s;
|
||||
border-radius: 34px;
|
||||
}
|
||||
|
||||
/* Rounded sliders */
|
||||
.slider.round {
|
||||
border-radius: 34px;
|
||||
}
|
||||
|
||||
/* Slider styling when toggled */
|
||||
.slider:before {
|
||||
position: absolute;
|
||||
content: "";
|
||||
height: 26px;
|
||||
width: 26px;
|
||||
left: 4px;
|
||||
bottom: 4px;
|
||||
background-color: white;
|
||||
-webkit-transition: .4s;
|
||||
transition: .4s;
|
||||
border-radius: 50%;
|
||||
}
|
||||
|
||||
input:checked + .slider {
|
||||
background-color: #2196F3;
|
||||
}
|
||||
|
||||
input:focus + .slider {
|
||||
box-shadow: 0 0 1px #2196F3;
|
||||
}
|
||||
|
||||
input:checked + .slider:before {
|
||||
-webkit-transform: translateX(26px);
|
||||
-ms-transform: translateX(26px);
|
||||
transform: translateX(26px);
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div class="toggle-container">
|
||||
<h2>Stepper Control</h2>
|
||||
<label class="toggle-switch">
|
||||
<input type="checkbox" id="toggleReset" onclick="toggleReset()">
|
||||
<span class="slider round"></span>
|
||||
</label>
|
||||
<input type='range' class='slider1' id='slider' min='-100' max='100' value='0' oninput='updateSlider()' onpointerup='checkReset()'>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
var resetEnabled = false;
|
||||
|
||||
function updateSlider() {
|
||||
var sliderValue = document.getElementById('slider').value;
|
||||
var xhr = new XMLHttpRequest();
|
||||
xhr.open('POST', '/update', true);
|
||||
xhr.setRequestHeader('Content-Type', 'application/x-www-form-urlencoded');
|
||||
xhr.send('slider=' + sliderValue);
|
||||
}
|
||||
|
||||
function toggleReset() {
|
||||
var toggleCheckbox = document.getElementById('toggleReset');
|
||||
|
||||
if (toggleCheckbox.checked) {
|
||||
// Reset slider to middle position if toggle is checked
|
||||
resetSlider();
|
||||
}
|
||||
|
||||
resetEnabled = toggleCheckbox.checked;
|
||||
}
|
||||
|
||||
function checkReset() {
|
||||
if (resetEnabled) {
|
||||
resetSlider(); // Reset slider if enabled
|
||||
}
|
||||
}
|
||||
|
||||
function resetSlider() {
|
||||
document.getElementById('slider').value = 0; // Reset to middle position
|
||||
updateSlider(); // Trigger update
|
||||
}
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
)rawliteral";
|
||||
|
||||
|
||||
|
||||
/////////////////////////////////////
|
||||
//// Better looking but slow for some reason
|
||||
////////////////////////////////////
|
||||
|
||||
|
||||
//const char index_html[] PROGMEM = R"rawliteral(
|
||||
//<!DOCTYPE html>
|
||||
//<html>
|
||||
//<head>
|
||||
// <title>Slider Position</title>
|
||||
// <style>
|
||||
// /* Style the toggle switch container */
|
||||
// .toggle-container {
|
||||
// display: flex;
|
||||
// flex-direction: column;
|
||||
// align-items: center;
|
||||
// margin-bottom: 20px; /* Adjust spacing between checkbox and slider */
|
||||
// }
|
||||
//
|
||||
// /* Style the toggle switch */
|
||||
// .toggle-switch {
|
||||
// position: relative;
|
||||
// display: inline-block;
|
||||
// width: 60px;
|
||||
// height: 34px;
|
||||
// }
|
||||
//
|
||||
// /* Hide the default checkbox */
|
||||
// .toggle-switch input {
|
||||
// opacity: 0;
|
||||
// width: 0;
|
||||
// height: 0;
|
||||
// }
|
||||
//
|
||||
// /* The slider */
|
||||
// .slider {
|
||||
// position: absolute;
|
||||
// cursor: pointer;
|
||||
// top: 0;
|
||||
// left: 0;
|
||||
// right: 0;
|
||||
// bottom: 0;
|
||||
// background-color: #ccc;
|
||||
// -webkit-transition: .4s;
|
||||
// transition: .4s;
|
||||
// border-radius: 34px;
|
||||
// }
|
||||
//
|
||||
// /* Rounded sliders */
|
||||
// .slider.round {
|
||||
// border-radius: 34px;
|
||||
// }
|
||||
//
|
||||
// /* Slider styling when toggled */
|
||||
// .slider:before {
|
||||
// position: absolute;
|
||||
// content: "";
|
||||
// height: 26px;
|
||||
// width: 26px;
|
||||
// left: 4px;
|
||||
// bottom: 4px;
|
||||
// background-color: white;
|
||||
// -webkit-transition: .4s;
|
||||
// transition: .4s;
|
||||
// border-radius: 50%;
|
||||
// }
|
||||
//
|
||||
// input:checked + .slider {
|
||||
// background-color: #2196F3;
|
||||
// }
|
||||
//
|
||||
// input:focus + .slider {
|
||||
// box-shadow: 0 0 1px #2196F3;
|
||||
// }
|
||||
//
|
||||
// input:checked + .slider:before {
|
||||
// -webkit-transform: translateX(26px);
|
||||
// -ms-transform: translateX(26px);
|
||||
// transform: translateX(26px);
|
||||
// }
|
||||
// </style>
|
||||
//</head>
|
||||
//<body>
|
||||
// <div class="toggle-container">
|
||||
// <label class="toggle-switch">
|
||||
// <input type="checkbox" id="toggleReset" onclick="toggleReset()">
|
||||
// <span class="slider round"></span>
|
||||
// </label>
|
||||
// <input type='range' id='slider' min='-100' max='100' value='0' oninput='updateSlider()' onpointerup='checkReset()'>
|
||||
// </div>
|
||||
//
|
||||
// <script>
|
||||
// var resetEnabled = false;
|
||||
//
|
||||
// function updateSlider() {
|
||||
// var sliderValue = document.getElementById('slider').value;
|
||||
// var xhr = new XMLHttpRequest();
|
||||
// xhr.open('POST', '/update', true);
|
||||
// xhr.setRequestHeader('Content-Type', 'application/x-www-form-urlencoded');
|
||||
// xhr.send('slider=' + sliderValue);
|
||||
// }
|
||||
//
|
||||
// function toggleReset() {
|
||||
// var toggleCheckbox = document.getElementById('toggleReset');
|
||||
//
|
||||
// if (toggleCheckbox.checked) {
|
||||
// // Reset slider to middle position if toggle is checked
|
||||
// resetSlider();
|
||||
// }
|
||||
//
|
||||
// resetEnabled = toggleCheckbox.checked;
|
||||
// }
|
||||
//
|
||||
// function checkReset() {
|
||||
// if (resetEnabled) {
|
||||
// resetSlider(); // Reset slider if enabled
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// function resetSlider() {
|
||||
// document.getElementById('slider').value = 0; // Reset to middle position
|
||||
// updateSlider(); // Trigger update
|
||||
// }
|
||||
// </script>
|
||||
//</body>
|
||||
//</html>
|
||||
//)rawliteral";
|
Loading…
Reference in New Issue
Block a user