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

store past locations to EEPROM and add homing logic

This commit is contained in:
Hunter 2025-06-03 13:57:29 -04:00
parent 5c7d3be63d
commit 3c456f6875
1 changed files with 179 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

267
src/main.cpp
View File

@ -10,7 +10,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;
#define EEPROM_SIZE 4 #define EEPROM_SIZE 18
#define EEPROM_BOTTOM_POS_ADDR 0
#define EEPROM_TOP_POS_ADDR 4
#define EEPROM_FLAG_ADDR 8
#define EEPROM_BOTTOM_MOVING_FLAG_ADDR 9
#define EEPROM_TOP_MOVING_FLAG_ADDR 10
#define EEPROM_TOP_BOTTOM_DIFF_ADDR 11
#define HOMING_FLAG 0x01
#define NORMAL_FLAG 0x00
#define MOVING_FLAG_SET 0xAA
#define MOVING_FLAG_CLEAR 0x00
const char *ssid = "IOT-pecan"; const char *ssid = "IOT-pecan";
const char *password = "aaaaaaaa"; const char *password = "aaaaaaaa";
@ -19,13 +29,14 @@ const int mqttPort = 1883;
const char *mqttHeightTopic = "/jc/height"; const char *mqttHeightTopic = "/jc/height";
const char *mqttAngleTopic = "/jc/angle"; const char *mqttAngleTopic = "/jc/angle";
const char *mqttStopTopic = "/jc/stop"; const char *mqttStopTopic = "/jc/stop";
const char *mqttHomingFlagTopic = "/jc/homing_mode";
const char *mqttDebugTopic = "/jc/debug";
const char *mqttStatusTopic = "/jc/status";
const char *mqttHeightLogTopic = "/jc/height/log"; const char *mqttHeightLogTopic = "/jc/height/log";
const char *mqttAngleLogTopic = "/jc/angle/log"; const char *mqttAngleLogTopic = "/jc/angle/log";
const char *mqttRPMLogTopic = "/jc/rpm/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; const int steps_per_thou = 22;
FastAccelStepperEngine engine = FastAccelStepperEngine(); FastAccelStepperEngine engine = FastAccelStepperEngine();
@ -37,49 +48,86 @@ 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);
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 stop = false;
bool system_locked = false;
void setup() volatile int top_angle_difference_steps = EEPROM.read(EEPROM_TOP_BOTTOM_DIFF_ADDR);
void writePositionToEEPROM(int addr, long pos)
{ {
Serial.begin(9600); EEPROM.write(addr, (pos >> 24) & 0xFF);
EEPROM.begin(EEPROM_SIZE); EEPROM.write(addr + 1, (pos >> 16) & 0xFF);
engine.init(); EEPROM.write(addr + 2, (pos >> 8) & 0xFF);
bottom_stepper = engine.stepperConnectToPin(bottom_step_pin); EEPROM.write(addr + 3, pos & 0xFF);
if (bottom_stepper) }
long readPositionFromEEPROM(int addr)
{
long pos = ((long)EEPROM.read(addr) << 24) |
((long)EEPROM.read(addr + 1) << 16) |
((long)EEPROM.read(addr + 2) << 8) |
(long)EEPROM.read(addr + 3);
return pos;
}
void callback(char *topic, byte *payload, unsigned int length)
{
String topic_str = String(topic);
String payload_str = "";
for (unsigned int i = 0; i < length; i++)
payload_str += (char)payload[i];
int value = payload_str.toInt();
if (topic_str == mqttHeightTopic && !system_locked)
{ {
bottom_stepper->setDirectionPin(bottom_dir_pin); value -= 1875 - 210;
bottom_stepper->setEnablePin(bottom_en_pin, false); value *= steps_per_thou;
bottom_stepper->setSpeedInHz(1000); stop = false;
bottom_stepper->setAcceleration(250); bottom_step_goal = value;
bottom_stepper->setCurrentPosition(EEPROM.read(0) << 8 | EEPROM.read(1)); bottom_step_done = false;
bottom_step_sent = false;
top_step_goal = value + top_angle_difference_steps;
top_step_done = false;
top_step_sent = false;
} }
top_stepper = engine.stepperConnectToPin(top_step_pin); else if (topic_str == mqttAngleTopic && !system_locked)
if (top_stepper)
{ {
top_stepper->setDirectionPin(top_dir_pin); stop = false;
top_stepper->setEnablePin(top_en_pin, false); top_angle_difference_steps = value * steps_per_thou;
top_stepper->setSpeedInHz(1000); top_step_goal = bottom_step_goal + top_angle_difference_steps;
top_stepper->setAcceleration(250); top_step_done = false;
top_stepper->setCurrentPosition(EEPROM.read(2) << 8 | EEPROM.read(3)); top_step_sent = false;
}
else if (topic_str == mqttStopTopic && !system_locked)
{
stop = true;
}
else if (topic_str == mqttHomingFlagTopic)
{
if (payload_str != "0")
{
EEPROM.write(EEPROM_FLAG_ADDR, HOMING_FLAG);
mqttClient.publish(mqttDebugTopic, "Startup flag set to HOMING");
}
else
{
EEPROM.write(EEPROM_FLAG_ADDR, NORMAL_FLAG);
mqttClient.publish(mqttDebugTopic, "Startup flag set to NORMAL");
}
EEPROM.commit();
} }
WiFi.begin(ssid, password); if (top_step_goal < -1665 * steps_per_thou)
while (WiFi.status() != WL_CONNECTED) top_step_goal = -1665 * steps_per_thou;
{ if (bottom_step_goal < -1665 * steps_per_thou)
delay(1000); bottom_step_goal = -1665 * steps_per_thou;
Serial.println("Connecting to WiFi..."); if (top_step_goal > 0)
} top_step_goal = 0;
Serial.println("Connected to WiFi"); if (bottom_step_goal > 0)
bottom_step_goal = 0;
mqttClient.setServer(mqttServer, mqttPort);
mqttClient.setCallback(callback);
Serial.println("Connected to MQTT");
} }
void reconnect() void reconnect()
@ -94,6 +142,8 @@ void reconnect()
Serial.println("connected"); Serial.println("connected");
mqttClient.subscribe(mqttHeightTopic); mqttClient.subscribe(mqttHeightTopic);
mqttClient.subscribe(mqttAngleTopic); mqttClient.subscribe(mqttAngleTopic);
mqttClient.subscribe(mqttStopTopic);
mqttClient.subscribe(mqttHomingFlagTopic);
} }
else else
{ {
@ -105,69 +155,97 @@ void reconnect()
} }
} }
volatile int top_angle_difference_steps = steps_per_thou * 188; long last_publish_time = 0;
volatile bool stop = false;
void callback(char *topic, byte *payload, unsigned int length) void setup()
{ {
String topic_str = String(topic); Serial.begin(9600);
String payload_str = ""; EEPROM.begin(EEPROM_SIZE);
for (int i = 0; i < length; i++) engine.init();
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED)
{ {
payload_str += (char)payload[i]; delay(1000);
Serial.println("Connecting to WiFi...");
}
Serial.println("Connected to WiFi");
mqttClient.setServer(mqttServer, mqttPort);
mqttClient.setCallback(callback);
uint8_t startupFlag = EEPROM.read(EEPROM_FLAG_ADDR);
Serial.print("Startup flag: ");
Serial.println(startupFlag);
uint8_t bottomMovingFlag = EEPROM.read(EEPROM_BOTTOM_MOVING_FLAG_ADDR);
uint8_t topMovingFlag = EEPROM.read(EEPROM_TOP_MOVING_FLAG_ADDR);
if (startupFlag != HOMING_FLAG && (bottomMovingFlag != MOVING_FLAG_CLEAR || topMovingFlag != MOVING_FLAG_CLEAR))
{
system_locked = true;
mqttClient.publish(mqttDebugTopic, "FAULT: NEEDS HOMING RESET");
Serial.println("System locked: Improper shutdown detected. Awaiting homing reset.");
EEPROM.write(EEPROM_FLAG_ADDR, HOMING_FLAG);
EEPROM.commit();
} }
int value = payload_str.toInt(); bottom_stepper = engine.stepperConnectToPin(bottom_step_pin);
if (bottom_stepper)
{
bottom_stepper->setDirectionPin(bottom_dir_pin);
bottom_stepper->setEnablePin(bottom_en_pin, false);
bottom_stepper->setSpeedInHz(1000);
bottom_stepper->setAcceleration(250);
if (startupFlag == HOMING_FLAG)
{
Serial.println("Starting in homing mode (bottom)");
bottom_stepper->setCurrentPosition(0);
writePositionToEEPROM(EEPROM_BOTTOM_POS_ADDR, bottom_stepper->getCurrentPosition());
EEPROM.write(EEPROM_FLAG_ADDR, NORMAL_FLAG);
EEPROM.write(EEPROM_BOTTOM_MOVING_FLAG_ADDR, MOVING_FLAG_CLEAR);
EEPROM.commit();
mqttClient.publish(mqttDebugTopic, "Started in HOMING mode, flag cleared");
}
else
{
bottom_stepper->setCurrentPosition(readPositionFromEEPROM(EEPROM_BOTTOM_POS_ADDR));
mqttClient.publish(mqttDebugTopic, "Started in NORMAL mode");
mqttClient.publish(mqttDebugTopic, "Bottom stepper loaded %d position from EEPROM", bottom_stepper->getCurrentPosition());
}
}
if (topic_str == mqttHeightTopic) top_stepper = engine.stepperConnectToPin(top_step_pin);
if (top_stepper)
{ {
value -= 1875 - 210; top_stepper->setDirectionPin(top_dir_pin);
value *= steps_per_thou; top_stepper->setEnablePin(top_en_pin, false);
Serial.println(value); top_stepper->setSpeedInHz(1000);
stop = false; top_stepper->setAcceleration(250);
bottom_step_goal = value; if (startupFlag == HOMING_FLAG)
bottom_step_done = false; {
bottom_step_sent = false; Serial.println("Starting in homing mode (top)");
top_step_goal = value + top_angle_difference_steps; top_stepper->setCurrentPosition(0);
top_step_done = false; writePositionToEEPROM(EEPROM_TOP_POS_ADDR, top_stepper->getCurrentPosition());
top_step_sent = false; EEPROM.write(EEPROM_TOP_MOVING_FLAG_ADDR, MOVING_FLAG_CLEAR);
EEPROM.commit();
}
else
{
top_stepper->setCurrentPosition(readPositionFromEEPROM(EEPROM_TOP_POS_ADDR));
mqttClient.publish(mqttDebugTopic, "Top stepper loaded %d position from EEPROM", top_stepper->getCurrentPosition());
}
} }
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)
{
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 > 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() void loop()
{ {
if (!mqttClient.connected()) if (!mqttClient.connected())
{
reconnect(); reconnect();
}
mqttClient.loop(); mqttClient.loop();
encoder.tick(); encoder.tick();
if (unsigned long elapsed = (millis() - last_publish_time) > 1000) if (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());
@ -175,6 +253,13 @@ void loop()
mqttClient.publish(mqttRPMLogTopic, String(encoder.getRPM()).c_str()); mqttClient.publish(mqttRPMLogTopic, String(encoder.getRPM()).c_str());
} }
if (system_locked)
{
bottom_stepper->disableOutputs();
top_stepper->disableOutputs();
return;
}
if (stop) if (stop)
{ {
bottom_stepper->disableOutputs(); bottom_stepper->disableOutputs();
@ -190,6 +275,8 @@ void loop()
{ {
bottom_stepper->moveTo(bottom_step_goal); bottom_stepper->moveTo(bottom_step_goal);
bottom_step_sent = true; bottom_step_sent = true;
EEPROM.write(EEPROM_BOTTOM_MOVING_FLAG_ADDR, MOVING_FLAG_SET);
EEPROM.commit();
Serial.println("Moving bottom stepper"); Serial.println("Moving bottom stepper");
} }
@ -197,6 +284,8 @@ void loop()
{ {
top_stepper->moveTo(top_step_goal); top_stepper->moveTo(top_step_goal);
top_step_sent = true; top_step_sent = true;
EEPROM.write(EEPROM_TOP_MOVING_FLAG_ADDR, MOVING_FLAG_SET);
EEPROM.commit();
Serial.println("Moving top stepper"); Serial.println("Moving top stepper");
} }
@ -204,8 +293,10 @@ void loop()
{ {
bottom_step_done = true; bottom_step_done = true;
bottom_step_sent = false; bottom_step_sent = false;
EEPROM.write(0, bottom_stepper->getCurrentPosition() >> 8); writePositionToEEPROM(EEPROM_BOTTOM_POS_ADDR, bottom_stepper->getCurrentPosition());
EEPROM.write(1, bottom_stepper->getCurrentPosition() & 0xFF); EEPROM.write(EEPROM_TOP_BOTTOM_DIFF_ADDR, top_angle_difference_steps);
EEPROM.write(EEPROM_BOTTOM_MOVING_FLAG_ADDR, MOVING_FLAG_CLEAR);
EEPROM.commit();
Serial.println("Bottom stepper done"); Serial.println("Bottom stepper done");
} }
@ -213,10 +304,10 @@ void loop()
{ {
top_step_done = true; top_step_done = true;
top_step_sent = false; top_step_sent = false;
EEPROM.write(2, top_stepper->getCurrentPosition() >> 8); writePositionToEEPROM(EEPROM_TOP_POS_ADDR, top_stepper->getCurrentPosition());
EEPROM.write(3, top_stepper->getCurrentPosition() & 0xFF); EEPROM.write(EEPROM_TOP_BOTTOM_DIFF_ADDR, top_angle_difference_steps);
EEPROM.write(EEPROM_TOP_MOVING_FLAG_ADDR, MOVING_FLAG_CLEAR);
EEPROM.commit();
Serial.println("Top stepper done"); Serial.println("Top stepper done");
} }
}
EEPROM.commit();
}