/* Electronic Lead screw By Antti Logren
VERSION ELS_9.3 - Fixed-Point Math Optimization & 1s RPM Stability
*/
#include <ModbusRTUSlave.h>
#include <ESP32Encoder.h>
#include <Preferences.h>
// --- PINS ---
#define ENCODER_A 10
#define ENCODER_B 11
#define STEP_PIN 41
#define DIR_PIN 40
#define EN_PIN 42
#define MODBUS_TX 13
#define MODBUS_RX 14
#define BUTTON_PIN 48
#define POT_PIN 18
ESP32Encoder encoder;
ModbusRTUSlave modbus(Serial1);
Preferences prefs;
enum SystemState { STATE_IDLE, STATE_FEED, STATE_THREAD, STATE_IMPERIAL };
volatile SystemState currentState = STATE_IDLE;
// --- FIXED POINT SCALING ---
const uint32_t SHIFT_FACTOR = 16;
const uint32_t SCALE_VALUE = (1 << SHIFT_FACTOR); // 65536
// --- MODBUS MEMORY MAP ---
bool coils[9] = {false, false, false, false, false, false, false, false, false};
bool discreteInputs[9] = {false, false, false, false, false, false, false, false, false};
uint16_t holdingRegisters[11] = {1, 25, 150, 1440, 3175, 400, 400, 8, 0, 0, 0};
uint16_t inputRegisters[11] = {0, 0, 0, 1440, 3175, 400, 400, 0, 0, 0, 0};
// --- SHARED VARIABLES ---
volatile uint32_t stepsPerCountFixed = 0; // Fixed-point ratio
volatile long lastPosition = 0;
float encoderCPR = 1440.0;
float leadscrewPitch_mm = 3.175;
float leadscrewTPI = 8.0;
float stepperStepsPerRev = 400.0;
float stepperGearReduction = 4.0;
bool isImperialLeadscrew = true;
unsigned long lastRPMCheck = 0;
long lastEncoderCountRPM = 0;
const uint16_t threadTable[] = {35, 40, 45, 50, 60, 70, 75, 80, 90, 100, 125, 150, 175, 200, 250, 300};
const uint16_t tpiTable[] = {3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 16, 18, 20, 24, 28, 32};
TaskHandle_t MotionTask;
// --- UNIVERSAL RATIO CALCULATIONS ---
void updateRatioMetric(float target_mm) {
if (encoderCPR <= 0) return;
float ratio = (target_mm / leadscrewPitch_mm) * (stepperStepsPerRev / encoderCPR) * stepperGearReduction;
stepsPerCountFixed = (uint32_t)(ratio * SCALE_VALUE);
}
void updateRatioImperial(float target_tpi) {
if (encoderCPR <= 0 || target_tpi <= 0) return;
float ratio;
if (isImperialLeadscrew) {
ratio = (leadscrewTPI / target_tpi) * (stepperStepsPerRev / encoderCPR) * stepperGearReduction;
} else {
float target_mm = 25.4 / target_tpi;
ratio = (target_mm / leadscrewPitch_mm) * (stepperStepsPerRev / encoderCPR) * stepperGearReduction;
}
stepsPerCountFixed = (uint32_t)(ratio * SCALE_VALUE);
}
// --- MOTION TASK (CORE 1) ---
void motionLoop(void * pvParameters) {
uint32_t stepAccumulator = 0;
for(;;) {
if (currentState != STATE_IDLE && stepsPerCountFixed > 0) {
long currentPosition = encoder.getCount();
if (currentPosition != lastPosition) {
long deltaCounts = currentPosition - lastPosition;
lastPosition = currentPosition;
bool finalDir = (deltaCounts > 0 ? LOW : HIGH) ^ discreteInputs[3];
digitalWrite(DIR_PIN, finalDir);
// Fixed point accumulation: No float math in the hot loop
stepAccumulator += abs(deltaCounts) * stepsPerCountFixed;
// While accumulator is greater than or equal to our scale (1.0)
while (stepAccumulator >= SCALE_VALUE) {
digitalWrite(STEP_PIN, HIGH);
ets_delay_us(2); // Reduced for higher frequency capability
digitalWrite(STEP_PIN, LOW);
ets_delay_us(2);
stepAccumulator -= SCALE_VALUE;
}
}
} else {
lastPosition = encoder.getCount();
stepAccumulator = 0;
}
yield();
}
}
void setup() {
Serial.begin(115200);
Serial1.begin(9600, SERIAL_8N1, MODBUS_RX, MODBUS_TX);
prefs.begin("els_params", false);
encoderCPR = prefs.getFloat("cpr", 1440.0);
leadscrewPitch_mm = prefs.getFloat("pitch", 3.175);
stepperStepsPerRev = prefs.getFloat("steps", 400.0);
stepperGearReduction = prefs.getFloat("gear", 4.0);
isImperialLeadscrew = prefs.getBool("is_imp", true);
leadscrewTPI = 25.4 / leadscrewPitch_mm;
// Sync Modbus UI
coils[8] = isImperialLeadscrew;
discreteInputs[8] = isImperialLeadscrew;
inputRegisters[3] = (uint16_t)encoderCPR;
inputRegisters[4] = isImperialLeadscrew ? (uint16_t)leadscrewTPI : (uint16_t)(leadscrewPitch_mm * 1000.0);
inputRegisters[5] = (uint16_t)stepperStepsPerRev;
inputRegisters[6] = (uint16_t)(stepperGearReduction * 100.0);
holdingRegisters[3] = inputRegisters[3];
holdingRegisters[4] = inputRegisters[4];
holdingRegisters[5] = inputRegisters[5];
holdingRegisters[6] = inputRegisters[6];
encoder.attachFullQuad(ENCODER_A, ENCODER_B);
pinMode(STEP_PIN, OUTPUT); pinMode(DIR_PIN, OUTPUT); pinMode(EN_PIN, OUTPUT);
digitalWrite(EN_PIN, HIGH);
pinMode(BUTTON_PIN, INPUT_PULLUP);
modbus.configureCoils(coils, 9);
modbus.configureDiscreteInputs(discreteInputs, 9);
modbus.configureHoldingRegisters(holdingRegisters, 11);
modbus.configureInputRegisters(inputRegisters, 11);
modbus.begin(1, 9600);
xTaskCreatePinnedToCore(motionLoop, "MotionTask", 10000, NULL, 1, &MotionTask, 1);
}
void loop() {
modbus.poll();
uint16_t pageID = holdingRegisters[0];
// --- 1. SETTINGS LOGIC ---
if (pageID >= 5 && pageID <= 8) {
if (coils[3]) {
switch(pageID) {
case 5: encoderCPR = (float)holdingRegisters[3]; prefs.putFloat("cpr", encoderCPR); inputRegisters[3] = holdingRegisters[3]; break;
case 6:
isImperialLeadscrew = coils[8];
if (isImperialLeadscrew) { leadscrewTPI = (float)holdingRegisters[4]; leadscrewPitch_mm = 25.4 / leadscrewTPI; }
else { leadscrewPitch_mm = (float)holdingRegisters[4] / 1000.0; leadscrewTPI = 25.4 / leadscrewPitch_mm; }
prefs.putFloat("pitch", leadscrewPitch_mm); prefs.putBool("is_imp", isImperialLeadscrew);
inputRegisters[4] = holdingRegisters[4]; discreteInputs[8] = isImperialLeadscrew; break;
case 7: stepperStepsPerRev = (float)holdingRegisters[5]; prefs.putFloat("steps", stepperStepsPerRev); inputRegisters[5] = holdingRegisters[5]; break;
case 8: stepperGearReduction = (float)holdingRegisters[6] / 100.0; prefs.putFloat("gear", stepperGearReduction); inputRegisters[6] = holdingRegisters[6]; break;
}
discreteInputs[6] = true; coils[3] = false;
}
} else { discreteInputs[6] = false; }
// --- 2. RPM CALC (1s intervals) ---
if (pageID == 9) {
unsigned long now = millis();
if (now - lastRPMCheck >= 1000) {
long currentCount = encoder.getCount();
long delta = abs(currentCount - lastEncoderCountRPM);
// RPM = (Delta * 60) / CPR
inputRegisters[10] = (uint16_t)((delta * 60.0) / encoderCPR);
lastEncoderCountRPM = currentCount;
lastRPMCheck = now;
}
}
// --- 3. HW DIRECTION & POTENTIOMETER ---
discreteInputs[3] = (digitalRead(BUTTON_PIN) == LOW);
if (coils[0]) {
int pot = analogRead(POT_PIN);
static int lastPot = 0;
if (abs(pot - lastPot) > 50) {
lastPot = pot;
if (pageID == 2) holdingRegisters[1] = (uint16_t)((map(pot, 0, 4095, 5, 100) + 2) / 5) * 5;
else if (pageID == 3) holdingRegisters[2] = threadTable[map(pot, 0, 4095, 0, 15)];
else if (pageID == 4) holdingRegisters[7] = tpiTable[map(pot, 0, 4095, 0, 17)];
}
}
// --- 4. STATE MACHINE ---
if (currentState == STATE_IDLE) {
if (pageID == 2 && coils[1]) { updateRatioMetric((float)holdingRegisters[1] / 100.0); currentState = STATE_FEED; digitalWrite(EN_PIN, LOW); discreteInputs[4] = true; }
else if (pageID == 3 && coils[2]) { updateRatioMetric((float)holdingRegisters[2] / 100.0); currentState = STATE_THREAD; digitalWrite(EN_PIN, LOW); discreteInputs[5] = true; }
else if (pageID == 4 && coils[4]) { updateRatioImperial((float)holdingRegisters[7]); currentState = STATE_IMPERIAL; digitalWrite(EN_PIN, LOW); discreteInputs[7] = true; }
} else {
if ((currentState == STATE_FEED && !coils[1]) || (currentState == STATE_THREAD && !coils[2]) || (currentState == STATE_IMPERIAL && !coils[4])) {
currentState = STATE_IDLE; digitalWrite(EN_PIN, HIGH); discreteInputs[4] = discreteInputs[5] = discreteInputs[7] = false;
}
}
}
).
