store past locations to EEPROM and add homing logic
This commit is contained in:
parent
5c7d3be63d
commit
3c456f6875
267
src/main.cpp
267
src/main.cpp
|
|
@ -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 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 *password = "aaaaaaaa";
|
||||
|
|
@ -19,13 +29,14 @@ const int mqttPort = 1883;
|
|||
const char *mqttHeightTopic = "/jc/height";
|
||||
const char *mqttAngleTopic = "/jc/angle";
|
||||
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 *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();
|
||||
|
|
@ -37,49 +48,86 @@ RotaryEncoder encoder(encoder_a, encoder_b);
|
|||
WiFiClient 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 bool bottom_step_done = true, top_step_done = true;
|
||||
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.begin(EEPROM_SIZE);
|
||||
engine.init();
|
||||
bottom_stepper = engine.stepperConnectToPin(bottom_step_pin);
|
||||
if (bottom_stepper)
|
||||
EEPROM.write(addr, (pos >> 24) & 0xFF);
|
||||
EEPROM.write(addr + 1, (pos >> 16) & 0xFF);
|
||||
EEPROM.write(addr + 2, (pos >> 8) & 0xFF);
|
||||
EEPROM.write(addr + 3, pos & 0xFF);
|
||||
}
|
||||
|
||||
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);
|
||||
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));
|
||||
value -= 1875 - 210;
|
||||
value *= steps_per_thou;
|
||||
stop = false;
|
||||
bottom_step_goal = value;
|
||||
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);
|
||||
if (top_stepper)
|
||||
else if (topic_str == mqttAngleTopic && !system_locked)
|
||||
{
|
||||
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));
|
||||
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 && !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);
|
||||
while (WiFi.status() != WL_CONNECTED)
|
||||
{
|
||||
delay(1000);
|
||||
Serial.println("Connecting to WiFi...");
|
||||
}
|
||||
Serial.println("Connected to WiFi");
|
||||
|
||||
mqttClient.setServer(mqttServer, mqttPort);
|
||||
mqttClient.setCallback(callback);
|
||||
|
||||
Serial.println("Connected to MQTT");
|
||||
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;
|
||||
}
|
||||
|
||||
void reconnect()
|
||||
|
|
@ -94,6 +142,8 @@ void reconnect()
|
|||
Serial.println("connected");
|
||||
mqttClient.subscribe(mqttHeightTopic);
|
||||
mqttClient.subscribe(mqttAngleTopic);
|
||||
mqttClient.subscribe(mqttStopTopic);
|
||||
mqttClient.subscribe(mqttHomingFlagTopic);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -105,69 +155,97 @@ void reconnect()
|
|||
}
|
||||
}
|
||||
|
||||
volatile int top_angle_difference_steps = steps_per_thou * 188;
|
||||
volatile bool stop = false;
|
||||
long last_publish_time = 0;
|
||||
|
||||
void callback(char *topic, byte *payload, unsigned int length)
|
||||
void setup()
|
||||
{
|
||||
String topic_str = String(topic);
|
||||
String payload_str = "";
|
||||
for (int i = 0; i < length; i++)
|
||||
Serial.begin(9600);
|
||||
EEPROM.begin(EEPROM_SIZE);
|
||||
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;
|
||||
value *= steps_per_thou;
|
||||
Serial.println(value);
|
||||
stop = false;
|
||||
bottom_step_goal = value;
|
||||
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->setDirectionPin(top_dir_pin);
|
||||
top_stepper->setEnablePin(top_en_pin, false);
|
||||
top_stepper->setSpeedInHz(1000);
|
||||
top_stepper->setAcceleration(250);
|
||||
if (startupFlag == HOMING_FLAG)
|
||||
{
|
||||
Serial.println("Starting in homing mode (top)");
|
||||
top_stepper->setCurrentPosition(0);
|
||||
writePositionToEEPROM(EEPROM_TOP_POS_ADDR, top_stepper->getCurrentPosition());
|
||||
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()
|
||||
{
|
||||
if (!mqttClient.connected())
|
||||
{
|
||||
reconnect();
|
||||
}
|
||||
mqttClient.loop();
|
||||
|
||||
encoder.tick();
|
||||
|
||||
if (unsigned long elapsed = (millis() - last_publish_time) > 1000)
|
||||
if (millis() - last_publish_time > 1000)
|
||||
{
|
||||
last_publish_time = millis();
|
||||
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());
|
||||
}
|
||||
|
||||
if (system_locked)
|
||||
{
|
||||
bottom_stepper->disableOutputs();
|
||||
top_stepper->disableOutputs();
|
||||
return;
|
||||
}
|
||||
|
||||
if (stop)
|
||||
{
|
||||
bottom_stepper->disableOutputs();
|
||||
|
|
@ -190,6 +275,8 @@ void loop()
|
|||
{
|
||||
bottom_stepper->moveTo(bottom_step_goal);
|
||||
bottom_step_sent = true;
|
||||
EEPROM.write(EEPROM_BOTTOM_MOVING_FLAG_ADDR, MOVING_FLAG_SET);
|
||||
EEPROM.commit();
|
||||
Serial.println("Moving bottom stepper");
|
||||
}
|
||||
|
||||
|
|
@ -197,6 +284,8 @@ void loop()
|
|||
{
|
||||
top_stepper->moveTo(top_step_goal);
|
||||
top_step_sent = true;
|
||||
EEPROM.write(EEPROM_TOP_MOVING_FLAG_ADDR, MOVING_FLAG_SET);
|
||||
EEPROM.commit();
|
||||
Serial.println("Moving top stepper");
|
||||
}
|
||||
|
||||
|
|
@ -204,8 +293,10 @@ void loop()
|
|||
{
|
||||
bottom_step_done = true;
|
||||
bottom_step_sent = false;
|
||||
EEPROM.write(0, bottom_stepper->getCurrentPosition() >> 8);
|
||||
EEPROM.write(1, bottom_stepper->getCurrentPosition() & 0xFF);
|
||||
writePositionToEEPROM(EEPROM_BOTTOM_POS_ADDR, bottom_stepper->getCurrentPosition());
|
||||
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");
|
||||
}
|
||||
|
||||
|
|
@ -213,10 +304,10 @@ void loop()
|
|||
{
|
||||
top_step_done = true;
|
||||
top_step_sent = false;
|
||||
EEPROM.write(2, top_stepper->getCurrentPosition() >> 8);
|
||||
EEPROM.write(3, top_stepper->getCurrentPosition() & 0xFF);
|
||||
writePositionToEEPROM(EEPROM_TOP_POS_ADDR, top_stepper->getCurrentPosition());
|
||||
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");
|
||||
}
|
||||
|
||||
EEPROM.commit();
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue