MODEL
/
usda-platformio
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

handle rotaray encoder and reconnect homing

This commit is contained in:
Hunter 2025-02-17 13:18:37 -05:00
parent 0358a39a59
commit 5c7d3be63d
2 changed files with 94 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
platformio.ini
View File

@ -9,11 +9,12 @@
; https://docs.platformio.org/page/projectconf.html ; https://docs.platformio.org/page/projectconf.html
[env:esp32] [env:esp32]
platform = espressif32 platform = espressif32 @ ^6.10.0
board = esp32dev board = esp32dev
framework = arduino framework = arduino
lib_deps = lib_deps =
FastAccelStepper FastAccelStepper
PubSubClient PubSubClient
ArduinoJson ArduinoJson
RotaryEncoder

172
src/main.cpp
View File

@ -2,92 +2,75 @@
#include <FastAccelStepper.h> #include <FastAccelStepper.h>
#include <WiFi.h> #include <WiFi.h>
#include <PubSubClient.h> #include <PubSubClient.h>
#include <RotaryEncoder.h>
#include <EEPROM.h>
const uint8_t bottom_dir_pin = 5, top_dir_pin = 18; const uint8_t bottom_dir_pin = 5, top_dir_pin = 18;
const uint8_t bottom_step_pin = 4, top_step_pin = 17; const uint8_t bottom_step_pin = 4, top_step_pin = 17;
const uint8_t bottom_en_pin = 12, top_en_pin = 16; const uint8_t bottom_en_pin = 12, top_en_pin = 16;
const uint8_t encoder_a = 19, encoder_b = 21; const uint8_t encoder_a = 19, encoder_b = 21;
const char* ssid = "IOT-pecan"; #define EEPROM_SIZE 4
const char* password = "aaaaaaaa";
const char* mqttServer = "192.168.1.110";
const int mqttPort = 1883;
const char* mqttHeightTopic = "/jc/height";
const char* mqttAngleTopic = "/jc/angle";
const char* mqttStopTopic = "/jc/stop";
const char* mqttHeightLogTopic = "/jc/height/log"; const char *ssid = "IOT-pecan";
const char* mqttAngleLogTopic = "/jc/angle/log"; const char *password = "aaaaaaaa";
const char* mqttRPMLogTopic = "/jc/rpm/log"; const char *mqttServer = "192.168.1.110";
const int mqttPort = 1883;
const char *mqttHeightTopic = "/jc/height";
const char *mqttAngleTopic = "/jc/angle";
const char *mqttStopTopic = "/jc/stop";
const char *mqttHeightLogTopic = "/jc/height/log";
const char *mqttAngleLogTopic = "/jc/angle/log";
const char *mqttRPMLogTopic = "/jc/rpm/log";
// Thread pitch: 11 TPI (0.0909 in/rev) // Thread pitch: 11 TPI (0.0909 in/rev)
// Steps per revolution: 2000; // Steps per revolution: 2000;
const int steps_per_thou = 22; const int steps_per_thou = 22;
FastAccelStepperEngine engine = FastAccelStepperEngine(); FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *bottom_stepper=nullptr; FastAccelStepper *bottom_stepper = nullptr;
FastAccelStepper *top_stepper=nullptr; FastAccelStepper *top_stepper = nullptr;
RotaryEncoder encoder(encoder_a, encoder_b);
WiFiClient wifiClient; WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient); PubSubClient mqttClient(wifiClient);
void callback(char* topic, byte* payload, unsigned int length); void callback(char *topic, byte *payload, unsigned int length);
volatile int encoder_count_a = 0, encoder_count_b = 0;
volatile int last_encoder_count_a = 0, last_encoder_count_b = 0;
volatile double rotations_since_last = 0;
volatile float encoder_rpm = 0;
uint16_t ticks_per_rotation = 600;
volatile long long bottom_step_goal = 0, top_step_goal = 0; volatile long long bottom_step_goal = 0, top_step_goal = 0;
volatile bool bottom_step_done = true, top_step_done = true; volatile bool bottom_step_done = true, top_step_done = true;
volatile bool bottom_step_sent = false, top_step_sent = false; volatile bool bottom_step_sent = false, top_step_sent = false;
volatile bool do_home = false; volatile bool do_home = false;
auto timer = timerBegin(0, 80, true); void setup()
{
void setup() {
Serial.begin(9600); Serial.begin(9600);
EEPROM.begin(EEPROM_SIZE);
engine.init(); engine.init();
bottom_stepper = engine.stepperConnectToPin(bottom_step_pin); bottom_stepper = engine.stepperConnectToPin(bottom_step_pin);
if (bottom_stepper) { if (bottom_stepper)
{
bottom_stepper->setDirectionPin(bottom_dir_pin); bottom_stepper->setDirectionPin(bottom_dir_pin);
bottom_stepper->setEnablePin(bottom_en_pin, false); bottom_stepper->setEnablePin(bottom_en_pin, false);
bottom_stepper->setSpeedInHz(1000); bottom_stepper->setSpeedInHz(1000);
bottom_stepper->setAcceleration(250); bottom_stepper->setAcceleration(250);
bottom_stepper->setCurrentPosition(EEPROM.read(0) << 8 | EEPROM.read(1));
} }
top_stepper = engine.stepperConnectToPin(top_step_pin); top_stepper = engine.stepperConnectToPin(top_step_pin);
if (top_stepper) { if (top_stepper)
{
top_stepper->setDirectionPin(top_dir_pin); top_stepper->setDirectionPin(top_dir_pin);
top_stepper->setEnablePin(top_en_pin, false); top_stepper->setEnablePin(top_en_pin, false);
top_stepper->setSpeedInHz(1000); top_stepper->setSpeedInHz(1000);
top_stepper->setAcceleration(250); top_stepper->setAcceleration(250);
top_stepper->setCurrentPosition(EEPROM.read(2) << 8 | EEPROM.read(3));
} }
pinMode(encoder_a, INPUT_PULLUP);
pinMode(encoder_b, INPUT_PULLUP);
attachInterrupt(encoder_a, []() {
encoder_count_a++;
double rotations = 0;
if(encoder_count_a > ticks_per_rotation / 100) {
rotations = (double)encoder_count_a / ticks_per_rotation;
rotations_since_last += rotations / 2;
encoder_count_a -= ticks_per_rotation * rotations;
}
}, FALLING);
attachInterrupt(encoder_b, []() {
encoder_count_b++;
double rotations = 0;
if(encoder_count_b > ticks_per_rotation / 100) {
rotations = (double)encoder_count_b / ticks_per_rotation;
rotations_since_last += rotations / 2;
encoder_count_b -= ticks_per_rotation * rotations;
}
}, FALLING);
WiFi.begin(ssid, password); WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED) { while (WiFi.status() != WL_CONNECTED)
{
delay(1000); delay(1000);
Serial.println("Connecting to WiFi..."); Serial.println("Connecting to WiFi...");
} }
@ -99,15 +82,21 @@ void setup() {
Serial.println("Connected to MQTT"); Serial.println("Connected to MQTT");
} }
void reconnect() { void reconnect()
while (!mqttClient.connected()) { {
if (WiFi.status() != WL_CONNECTED) WiFi.reconnect(); while (!mqttClient.connected())
{
if (WiFi.status() != WL_CONNECTED)
WiFi.reconnect();
Serial.println("Attempting MQTT connection..."); Serial.println("Attempting MQTT connection...");
if (mqttClient.connect("ESP32Client")) { if (mqttClient.connect("ESP32Client"))
{
Serial.println("connected"); Serial.println("connected");
mqttClient.subscribe(mqttHeightTopic); mqttClient.subscribe(mqttHeightTopic);
mqttClient.subscribe(mqttAngleTopic); mqttClient.subscribe(mqttAngleTopic);
} else { }
else
{
Serial.print("failed, rc="); Serial.print("failed, rc=");
Serial.print(mqttClient.state()); Serial.print(mqttClient.state());
Serial.println(" try again in 5 seconds"); Serial.println(" try again in 5 seconds");
@ -116,19 +105,22 @@ void reconnect() {
} }
} }
volatile int top_angle_difference_steps = steps_per_thou * 188; volatile int top_angle_difference_steps = steps_per_thou * 188;
volatile bool stop = false; volatile bool stop = false;
void callback(char* topic, byte* payload, unsigned int length) { void callback(char *topic, byte *payload, unsigned int length)
{
String topic_str = String(topic); String topic_str = String(topic);
String payload_str = ""; String payload_str = "";
for (int i = 0; i < length; i++) { for (int i = 0; i < length; i++)
{
payload_str += (char)payload[i]; payload_str += (char)payload[i];
} }
int value = payload_str.toInt(); int value = payload_str.toInt();
if (topic_str == mqttHeightTopic) { if (topic_str == mqttHeightTopic)
{
value -= 1875 - 210; value -= 1875 - 210;
value *= steps_per_thou; value *= steps_per_thou;
Serial.println(value); Serial.println(value);
@ -139,74 +131,92 @@ void callback(char* topic, byte* payload, unsigned int length) {
top_step_goal = value + top_angle_difference_steps; top_step_goal = value + top_angle_difference_steps;
top_step_done = false; top_step_done = false;
top_step_sent = false; top_step_sent = false;
} else if (topic_str == mqttAngleTopic) { }
else if (topic_str == mqttAngleTopic)
{
stop = false; stop = false;
top_angle_difference_steps = value * steps_per_thou; top_angle_difference_steps = value * steps_per_thou;
top_step_goal = bottom_step_goal + top_angle_difference_steps; top_step_goal = bottom_step_goal + top_angle_difference_steps;
top_step_done = false; top_step_done = false;
top_step_sent = false; top_step_sent = false;
} else if (topic_str == mqttStopTopic) { }
else if (topic_str == mqttStopTopic)
{
stop = true; stop = true;
} }
if (top_step_goal < -1665 * steps_per_thou) top_step_goal = -1665 * steps_per_thou; if (top_step_goal < -1665 * steps_per_thou)
if (bottom_step_goal < -1665 * steps_per_thou) bottom_step_goal = -1665 * steps_per_thou; top_step_goal = -1665 * steps_per_thou;
if (bottom_step_goal < -1665 * steps_per_thou)
bottom_step_goal = -1665 * steps_per_thou;
if(top_step_goal > 0) top_step_goal = 0; if (top_step_goal > 0)
if(bottom_step_goal > 0) bottom_step_goal = 0; top_step_goal = 0;
if (bottom_step_goal > 0)
bottom_step_goal = 0;
} }
long last_publish_time = 0; long last_publish_time = 0;
bool bot_homed = false, top_homed = false; bool bot_homed = false, top_homed = false;
void loop() { void loop()
if (!mqttClient.connected()) { {
if (!mqttClient.connected())
{
reconnect(); reconnect();
} }
mqttClient.loop(); mqttClient.loop();
if (unsigned long elapsed = (millis() - last_publish_time) > 1000) { encoder.tick();
encoder_rpm = (float)rotations_since_last * 60 / elapsed;
// clamp encoder rpm to 0 if below 50
if (encoder_rpm < 50) {
encoder_rpm = 0;
}
rotations_since_last = 0; if (unsigned long elapsed = (millis() - last_publish_time) > 1000)
{
last_publish_time = millis(); last_publish_time = millis();
mqttClient.publish(mqttHeightLogTopic, String(1665 + bottom_stepper->getCurrentPosition() / steps_per_thou).c_str()); mqttClient.publish(mqttHeightLogTopic, String(1665 + bottom_stepper->getCurrentPosition() / steps_per_thou).c_str());
mqttClient.publish(mqttAngleLogTopic, String((top_stepper->getCurrentPosition() - bottom_stepper->getCurrentPosition()) / steps_per_thou).c_str()); mqttClient.publish(mqttAngleLogTopic, String((top_stepper->getCurrentPosition() - bottom_stepper->getCurrentPosition()) / steps_per_thou).c_str());
mqttClient.publish(mqttRPMLogTopic, String(encoder_rpm).c_str()); mqttClient.publish(mqttRPMLogTopic, String(encoder.getRPM()).c_str());
} }
if(stop) { if (stop)
{
bottom_stepper->disableOutputs(); bottom_stepper->disableOutputs();
top_stepper->disableOutputs(); top_stepper->disableOutputs();
} else { }
else
{
bottom_stepper->enableOutputs(); bottom_stepper->enableOutputs();
top_stepper->enableOutputs(); top_stepper->enableOutputs();
} }
if(bottom_stepper->getCurrentPosition() != bottom_step_goal && !bottom_step_done && !bottom_step_sent) { if (bottom_stepper->getCurrentPosition() != bottom_step_goal && !bottom_step_done && !bottom_step_sent)
{
bottom_stepper->moveTo(bottom_step_goal); bottom_stepper->moveTo(bottom_step_goal);
bottom_step_sent = true; bottom_step_sent = true;
Serial.println("Moving bottom stepper"); Serial.println("Moving bottom stepper");
} }
if(top_stepper->getCurrentPosition() != top_step_goal && !top_step_done && !top_step_sent) { if (top_stepper->getCurrentPosition() != top_step_goal && !top_step_done && !top_step_sent)
{
top_stepper->moveTo(top_step_goal); top_stepper->moveTo(top_step_goal);
top_step_sent = true; top_step_sent = true;
Serial.println("Moving top stepper"); Serial.println("Moving top stepper");
} }
if(bottom_stepper->getCurrentPosition() == bottom_step_goal && !bottom_step_done && bottom_step_sent) { if (bottom_stepper->getCurrentPosition() == bottom_step_goal && !bottom_step_done && bottom_step_sent)
{
bottom_step_done = true; bottom_step_done = true;
bottom_step_sent = false; bottom_step_sent = false;
EEPROM.write(0, bottom_stepper->getCurrentPosition() >> 8);
EEPROM.write(1, bottom_stepper->getCurrentPosition() & 0xFF);
Serial.println("Bottom stepper done"); Serial.println("Bottom stepper done");
} }
if(top_stepper->getCurrentPosition() == top_step_goal && !top_step_done && top_step_sent) { if (top_stepper->getCurrentPosition() == top_step_goal && !top_step_done && top_step_sent)
{
top_step_done = true; top_step_done = true;
top_step_sent = false; top_step_sent = false;
EEPROM.write(2, top_stepper->getCurrentPosition() >> 8);
EEPROM.write(3, top_stepper->getCurrentPosition() & 0xFF);
Serial.println("Top stepper done"); Serial.println("Top stepper done");
} }
EEPROM.commit();
} }