handle rotaray encoder and reconnect homing
This commit is contained in:
parent
0358a39a59
commit
5c7d3be63d
|
|
@ -9,11 +9,12 @@
|
|||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:esp32]
|
||||
platform = espressif32
|
||||
platform = espressif32 @ ^6.10.0
|
||||
board = esp32dev
|
||||
framework = arduino
|
||||
lib_deps =
|
||||
lib_deps =
|
||||
FastAccelStepper
|
||||
PubSubClient
|
||||
ArduinoJson
|
||||
RotaryEncoder
|
||||
|
||||
|
|
|
|||
172
src/main.cpp
172
src/main.cpp
|
|
@ -2,92 +2,75 @@
|
|||
#include <FastAccelStepper.h>
|
||||
#include <WiFi.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_step_pin = 4, top_step_pin = 17;
|
||||
const uint8_t bottom_en_pin = 12, top_en_pin = 16;
|
||||
const uint8_t encoder_a = 19, encoder_b = 21;
|
||||
|
||||
const char* ssid = "IOT-pecan";
|
||||
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";
|
||||
#define EEPROM_SIZE 4
|
||||
|
||||
const char* mqttHeightLogTopic = "/jc/height/log";
|
||||
const char* mqttAngleLogTopic = "/jc/angle/log";
|
||||
const char* mqttRPMLogTopic = "/jc/rpm/log";
|
||||
const char *ssid = "IOT-pecan";
|
||||
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 *mqttAngleLogTopic = "/jc/angle/log";
|
||||
const char *mqttRPMLogTopic = "/jc/rpm/log";
|
||||
|
||||
// Thread pitch: 11 TPI (0.0909 in/rev)
|
||||
// Steps per revolution: 2000;
|
||||
const int steps_per_thou = 22;
|
||||
|
||||
FastAccelStepperEngine engine = FastAccelStepperEngine();
|
||||
FastAccelStepper *bottom_stepper=nullptr;
|
||||
FastAccelStepper *top_stepper=nullptr;
|
||||
FastAccelStepper *bottom_stepper = nullptr;
|
||||
FastAccelStepper *top_stepper = nullptr;
|
||||
|
||||
RotaryEncoder encoder(encoder_a, encoder_b);
|
||||
|
||||
WiFiClient wifiClient;
|
||||
PubSubClient mqttClient(wifiClient);
|
||||
|
||||
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;
|
||||
void callback(char *topic, byte *payload, unsigned int length);
|
||||
|
||||
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_sent = false, top_step_sent = false;
|
||||
volatile bool do_home = false;
|
||||
|
||||
auto timer = timerBegin(0, 80, true);
|
||||
|
||||
void setup() {
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(9600);
|
||||
EEPROM.begin(EEPROM_SIZE);
|
||||
engine.init();
|
||||
bottom_stepper = engine.stepperConnectToPin(bottom_step_pin);
|
||||
if (bottom_stepper) {
|
||||
if (bottom_stepper)
|
||||
{
|
||||
bottom_stepper->setDirectionPin(bottom_dir_pin);
|
||||
bottom_stepper->setEnablePin(bottom_en_pin, false);
|
||||
bottom_stepper->setSpeedInHz(1000);
|
||||
bottom_stepper->setAcceleration(250);
|
||||
bottom_stepper->setCurrentPosition(EEPROM.read(0) << 8 | EEPROM.read(1));
|
||||
}
|
||||
top_stepper = engine.stepperConnectToPin(top_step_pin);
|
||||
if (top_stepper) {
|
||||
if (top_stepper)
|
||||
{
|
||||
top_stepper->setDirectionPin(top_dir_pin);
|
||||
top_stepper->setEnablePin(top_en_pin, false);
|
||||
top_stepper->setSpeedInHz(1000);
|
||||
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);
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
while (WiFi.status() != WL_CONNECTED)
|
||||
{
|
||||
delay(1000);
|
||||
Serial.println("Connecting to WiFi...");
|
||||
}
|
||||
|
|
@ -99,15 +82,21 @@ void setup() {
|
|||
Serial.println("Connected to MQTT");
|
||||
}
|
||||
|
||||
void reconnect() {
|
||||
while (!mqttClient.connected()) {
|
||||
if (WiFi.status() != WL_CONNECTED) WiFi.reconnect();
|
||||
void reconnect()
|
||||
{
|
||||
while (!mqttClient.connected())
|
||||
{
|
||||
if (WiFi.status() != WL_CONNECTED)
|
||||
WiFi.reconnect();
|
||||
Serial.println("Attempting MQTT connection...");
|
||||
if (mqttClient.connect("ESP32Client")) {
|
||||
if (mqttClient.connect("ESP32Client"))
|
||||
{
|
||||
Serial.println("connected");
|
||||
mqttClient.subscribe(mqttHeightTopic);
|
||||
mqttClient.subscribe(mqttAngleTopic);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.print("failed, rc=");
|
||||
Serial.print(mqttClient.state());
|
||||
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;
|
||||
|
||||
void callback(char* topic, byte* payload, unsigned int length) {
|
||||
void callback(char *topic, byte *payload, unsigned int length)
|
||||
{
|
||||
String topic_str = String(topic);
|
||||
String payload_str = "";
|
||||
for (int i = 0; i < length; i++) {
|
||||
for (int i = 0; i < length; i++)
|
||||
{
|
||||
payload_str += (char)payload[i];
|
||||
}
|
||||
|
||||
int value = payload_str.toInt();
|
||||
|
||||
if (topic_str == mqttHeightTopic) {
|
||||
|
||||
if (topic_str == mqttHeightTopic)
|
||||
{
|
||||
value -= 1875 - 210;
|
||||
value *= steps_per_thou;
|
||||
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_done = false;
|
||||
top_step_sent = false;
|
||||
} else if (topic_str == mqttAngleTopic) {
|
||||
}
|
||||
else if (topic_str == mqttAngleTopic)
|
||||
{
|
||||
stop = false;
|
||||
top_angle_difference_steps = value * steps_per_thou;
|
||||
top_step_goal = bottom_step_goal + top_angle_difference_steps;
|
||||
top_step_done = false;
|
||||
top_step_sent = false;
|
||||
} else if (topic_str == mqttStopTopic) {
|
||||
}
|
||||
else if (topic_str == mqttStopTopic)
|
||||
{
|
||||
stop = true;
|
||||
}
|
||||
if (top_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 < -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(bottom_step_goal > 0) bottom_step_goal = 0;
|
||||
if (top_step_goal > 0)
|
||||
top_step_goal = 0;
|
||||
if (bottom_step_goal > 0)
|
||||
bottom_step_goal = 0;
|
||||
}
|
||||
|
||||
long last_publish_time = 0;
|
||||
bool bot_homed = false, top_homed = false;
|
||||
void loop() {
|
||||
if (!mqttClient.connected()) {
|
||||
void loop()
|
||||
{
|
||||
if (!mqttClient.connected())
|
||||
{
|
||||
reconnect();
|
||||
}
|
||||
mqttClient.loop();
|
||||
|
||||
if (unsigned long elapsed = (millis() - last_publish_time) > 1000) {
|
||||
encoder_rpm = (float)rotations_since_last * 60 / elapsed;
|
||||
|
||||
// clamp encoder rpm to 0 if below 50
|
||||
if (encoder_rpm < 50) {
|
||||
encoder_rpm = 0;
|
||||
}
|
||||
encoder.tick();
|
||||
|
||||
rotations_since_last = 0;
|
||||
if (unsigned long elapsed = (millis() - last_publish_time) > 1000)
|
||||
{
|
||||
last_publish_time = millis();
|
||||
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(mqttRPMLogTopic, String(encoder_rpm).c_str());
|
||||
mqttClient.publish(mqttRPMLogTopic, String(encoder.getRPM()).c_str());
|
||||
}
|
||||
|
||||
if(stop) {
|
||||
if (stop)
|
||||
{
|
||||
bottom_stepper->disableOutputs();
|
||||
top_stepper->disableOutputs();
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
bottom_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_step_sent = true;
|
||||
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_step_sent = true;
|
||||
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_sent = false;
|
||||
EEPROM.write(0, bottom_stepper->getCurrentPosition() >> 8);
|
||||
EEPROM.write(1, bottom_stepper->getCurrentPosition() & 0xFF);
|
||||
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_sent = false;
|
||||
EEPROM.write(2, top_stepper->getCurrentPosition() >> 8);
|
||||
EEPROM.write(3, top_stepper->getCurrentPosition() & 0xFF);
|
||||
Serial.println("Top stepper done");
|
||||
}
|
||||
|
||||
EEPROM.commit();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue