Stepper controller now done
This commit is contained in:
parent
76e516fa5d
commit
3e1418257a
|
|
@ -10,9 +10,9 @@
|
|||
// [X, Y, Head]
|
||||
#define NUM_STEPPERS 3
|
||||
Stepper steppers[NUM_STEPPERS] = {
|
||||
{38, 55, 54, 0, false, false, 'x'},
|
||||
{56, 61, 60, 0, false, false, 'y'},
|
||||
{30, 34, 36, 0, false, false, 'e'},
|
||||
{38, 55, 54, 0, 'x', false, false, true},
|
||||
{56, 61, 60, 0, 'y', false, false, true},
|
||||
{30, 34, 36, 0, 'e', false, false, true},
|
||||
};
|
||||
|
||||
#pragma region Not gonna mess with this magic
|
||||
|
|
@ -51,17 +51,17 @@ void IRAM_ATTR timer3ISR()
|
|||
|
||||
// Timers for stepping the motors
|
||||
ISR(TIMER3_COMPA_vect) {
|
||||
if (steppers[0].steppingEnabled) {
|
||||
if (steppers[0].steppingEnabled && steppers[0].moveQueue != 0) {
|
||||
digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin));
|
||||
}
|
||||
}
|
||||
ISR(TIMER4_COMPA_vect) {
|
||||
if (steppers[1].steppingEnabled) {
|
||||
if (steppers[1].steppingEnabled && steppers[1].moveQueue != 0) {
|
||||
digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin));
|
||||
}
|
||||
}
|
||||
ISR(TIMER5_COMPA_vect) {
|
||||
if (steppers[2].steppingEnabled) {
|
||||
if (steppers[2].steppingEnabled && steppers[2].moveQueue != 0) {
|
||||
digitalWrite(steppers[2].stepPin, !digitalRead(steppers[2].stepPin));
|
||||
}
|
||||
}
|
||||
|
|
@ -70,27 +70,29 @@ ISR(TIMER5_COMPA_vect) {
|
|||
#pragma endregion
|
||||
|
||||
void anotherFunc(void * params){
|
||||
MotorMoveParams *pam = (MotorMoveParams *)malloc(sizeof(MotorMoveParams));
|
||||
pam->motorChar = 'x';
|
||||
pam->moveTimeMs = 3000;
|
||||
// xTaskCreate(MoveMotor, "Motor", 1024, (void *)pam, 10, NULL);
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(50)); // Allow time for other task to accept params
|
||||
free(pam);
|
||||
while (true)
|
||||
{
|
||||
StepperController::SetMotorEnabled('x', true);
|
||||
StepperController::SetMovementEnabled('x', true);
|
||||
StepperController::QueueMove('x', 2000);
|
||||
Serial.println("Added to queue");
|
||||
vTaskDelay(pdMS_TO_TICKS(5000));
|
||||
}
|
||||
|
||||
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
||||
// setupMotor();
|
||||
|
||||
// Create tasks
|
||||
StepperController::Init(steppers, NUM_STEPPERS);
|
||||
|
||||
xTaskCreate(StepperController::MotorTask, "Motors", 1024, NULL, 5, NULL);
|
||||
xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5, NULL);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// The RTOS scheduler manages the tasks. No code is needed here.
|
||||
|
||||
// ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
|
||||
// The RTOS scheduler manages the tasks.
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,3 +1,6 @@
|
|||
#ifndef STEPPER_H
|
||||
#define STEPPER_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
struct Stepper {
|
||||
|
|
@ -5,7 +8,9 @@ struct Stepper {
|
|||
uint8_t dirPin;
|
||||
uint8_t stepPin;
|
||||
int moveQueue;
|
||||
char axis;
|
||||
bool steppingEnabled;
|
||||
bool motorEnabled;
|
||||
char axis;
|
||||
};
|
||||
bool disableOnIdle;
|
||||
};
|
||||
#endif
|
||||
|
|
@ -1,9 +1,16 @@
|
|||
#include "StepperController.h"
|
||||
|
||||
int StepperController::getStepperIndex(const char axis){
|
||||
for (size_t i = 0; i < NumOfSteppers; i++)
|
||||
// Static variable definitions
|
||||
int StepperController::NumOfSteppers = 0;
|
||||
Stepper *StepperController::Steppers = nullptr;
|
||||
long StepperController::lastUpdateTime = 0;
|
||||
|
||||
int StepperController::getStepperIndex(const char axis)
|
||||
{
|
||||
for (size_t i = 0; i < StepperController::NumOfSteppers; i++)
|
||||
{
|
||||
if (Steppers[i].axis == axis) {
|
||||
if (StepperController::Steppers[i].axis == axis)
|
||||
{
|
||||
// Serial.print("Found motor: ");
|
||||
// Serial.println(i);
|
||||
return i;
|
||||
|
|
@ -12,27 +19,123 @@ int StepperController::getStepperIndex(const char axis){
|
|||
return -1;
|
||||
}
|
||||
|
||||
void StepperController::SetMovementEnabled(const char axis, bool enabled) {
|
||||
int index = getStepperIndex(axis);
|
||||
Steppers[index].steppingEnabled = enabled;
|
||||
void StepperController::Init(Stepper *steppers, int numOfSteppers)
|
||||
{
|
||||
StepperController::Steppers = steppers;
|
||||
StepperController::NumOfSteppers = numOfSteppers;
|
||||
StepperController::lastUpdateTime = 0;
|
||||
|
||||
for (size_t i = 0; i < NumOfSteppers; i++)
|
||||
{
|
||||
pinMode(steppers[i].enPin, OUTPUT);
|
||||
pinMode(steppers[i].dirPin, OUTPUT);
|
||||
pinMode(steppers[i].stepPin, OUTPUT);
|
||||
}
|
||||
|
||||
#ifdef ESP32
|
||||
// Timer 1 - Equivalent to TIMER1_COMPA_vect
|
||||
timer1 = timerBegin(0, 80, true); // Timer 0, prescaler 80 (1us per tick)
|
||||
timerAttachInterrupt(timer1, &timer1ISR, true);
|
||||
timerAlarmWrite(timer1, 500, true); // 500us interval
|
||||
timerAlarmEnable(timer1);
|
||||
|
||||
// Timer 2 - Equivalent to TIMER3_COMPA_vect
|
||||
timer2 = timerBegin(1, 80, true); // Timer 1, prescaler 80 (1us per tick)
|
||||
timerAttachInterrupt(timer2, &timer2ISR, true);
|
||||
timerAlarmWrite(timer2, 500, true); // 500us interval
|
||||
timerAlarmEnable(timer2);
|
||||
|
||||
// Timer 3 - Equivalent to TIMER4_COMPA_vect
|
||||
timer3 = timerBegin(2, 80, true); // Timer 2, prescaler 80 (1us per tick)
|
||||
timerAttachInterrupt(timer3, &timer3ISR, true);
|
||||
timerAlarmWrite(timer3, 500, true); // 500us interval
|
||||
timerAlarmEnable(timer3);
|
||||
#else
|
||||
cli();
|
||||
// Timer 3 (16-bit)
|
||||
TCCR3A = 0;
|
||||
TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler
|
||||
OCR3A = 499; // Compare match value
|
||||
TIMSK3 |= (1 << OCIE3A); // Enable Timer1 Compare A Match Interrupt
|
||||
|
||||
// Timer 4 (16-bit)
|
||||
TCCR4A = 0;
|
||||
TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler
|
||||
OCR4A = 499; // Compare match value
|
||||
TIMSK4 |= (1 << OCIE4A); // Enable Timer3 Compare A Match Interrupt
|
||||
|
||||
// Timer 5 (16-bit)
|
||||
TCCR5A = 0;
|
||||
TCCR5B = (1 << WGM52) | (1 << CS50); // CTC mode, no prescaler
|
||||
OCR5A = 499; // Compare match value
|
||||
TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt
|
||||
sei();
|
||||
#endif
|
||||
}
|
||||
|
||||
void StepperController::SetMoveQueue(const char axis, int moveTimeMs) {
|
||||
int index = getStepperIndex(axis);
|
||||
Steppers[index].moveQueue = moveTimeMs;
|
||||
void StepperController::MotorTask(void *params)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
// Serial.println("Motor loop");
|
||||
long sinceLastRunTime = millis() - StepperController::lastUpdateTime;
|
||||
|
||||
for (size_t i = 0; i < (size_t)StepperController::NumOfSteppers; i++)
|
||||
{
|
||||
Stepper *stepper = &StepperController:: Steppers[i];
|
||||
bool isNegative = signbit(stepper->moveQueue);
|
||||
|
||||
if (stepper->steppingEnabled && stepper->moveQueue != 0)
|
||||
{
|
||||
if (stepper->moveQueue > 0) {
|
||||
stepper->moveQueue -= std::min((int)sinceLastRunTime, abs(stepper->moveQueue));
|
||||
} else {
|
||||
stepper->moveQueue += std::min((int)sinceLastRunTime, abs(stepper->moveQueue));
|
||||
}
|
||||
Serial.println(stepper->moveQueue);
|
||||
digitalWrite(stepper->dirPin, isNegative);
|
||||
|
||||
if (stepper->disableOnIdle && stepper->moveQueue == 0){
|
||||
stepper->motorEnabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Update motor enable states
|
||||
digitalWrite(stepper->enPin, !stepper->motorEnabled);
|
||||
}
|
||||
|
||||
StepperController::lastUpdateTime = millis();
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void StepperController::QueueMove(const char axis, int moveTimeMs) {
|
||||
int index = getStepperIndex(axis);
|
||||
Steppers[index].moveQueue += moveTimeMs; // Fixed incorrect `=+` operator
|
||||
void StepperController::SetMovementEnabled(const char axis, bool enabled)
|
||||
{
|
||||
int index = getStepperIndex(axis);
|
||||
StepperController::Steppers[index].steppingEnabled = enabled;
|
||||
}
|
||||
|
||||
void StepperController::ClearMoveQueue(const char axis) { // Fixed function name typo
|
||||
int index = getStepperIndex(axis);
|
||||
Steppers[index].moveQueue = 0;
|
||||
void StepperController::SetMoveQueue(const char axis, int moveTimeMs)
|
||||
{
|
||||
int index = getStepperIndex(axis);
|
||||
StepperController::Steppers[index].moveQueue = moveTimeMs;
|
||||
}
|
||||
|
||||
void StepperController::SetMotorEnabled(const char axis, bool enabled) {
|
||||
int index = getStepperIndex(axis);
|
||||
Steppers[index].motorEnabled = enabled;
|
||||
void StepperController::QueueMove(const char axis, int moveTimeMs)
|
||||
{
|
||||
int index = getStepperIndex(axis);
|
||||
StepperController::Steppers[index].moveQueue += moveTimeMs;
|
||||
}
|
||||
|
||||
void StepperController::ClearMoveQueue(const char axis)
|
||||
{
|
||||
int index = getStepperIndex(axis);
|
||||
StepperController::Steppers[index].moveQueue = 0;
|
||||
}
|
||||
|
||||
void StepperController::SetMotorEnabled(const char axis, bool enabled)
|
||||
{
|
||||
int index = getStepperIndex(axis);
|
||||
StepperController::Steppers[index].motorEnabled = enabled;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,85 +8,30 @@
|
|||
#include <Shared/ZCommand.h>
|
||||
#include <Shared/MotorMoveParams.h>
|
||||
#include <Shared/Steppers.h>
|
||||
#include <algorithm>
|
||||
|
||||
class StepperController
|
||||
{
|
||||
private:
|
||||
int NumOfSteppers;
|
||||
Stepper *Steppers;
|
||||
static int NumOfSteppers;
|
||||
static Stepper *Steppers;
|
||||
static long lastUpdateTime;
|
||||
|
||||
static int getStepperIndex(const char axis);
|
||||
|
||||
int getStepperIndex(const char axis);
|
||||
public:
|
||||
StepperController(Stepper *steppers, int numOfSteppers = 3);
|
||||
static void Init(Stepper *steppers, int numOfSteppers = 3);
|
||||
|
||||
void SetMovementEnabled(const char axis, bool enabled);
|
||||
static void MotorTask(void * params);
|
||||
|
||||
void SetMoveQueue(const char axis, const int moveTimeMs);
|
||||
void QueueMove(const char axis, const int moveTimeMs);
|
||||
void ClearMoveQueue(const char axis);
|
||||
static void SetMovementEnabled(const char axis, bool enabled);
|
||||
|
||||
void SetMotorEnabled(const char axis, bool enabled);
|
||||
static void SetMoveQueue(const char axis, const int moveTimeMs);
|
||||
static void QueueMove(const char axis, const int moveTimeMs);
|
||||
static void ClearMoveQueue(const char axis);
|
||||
|
||||
~StepperController();
|
||||
static void SetMotorEnabled(const char axis, bool enabled);
|
||||
};
|
||||
|
||||
StepperController::StepperController(Stepper steppers[], int numOfSteppers = 3)
|
||||
{
|
||||
Steppers = steppers;
|
||||
NumOfSteppers = numOfSteppers;
|
||||
|
||||
for (size_t i = 0; i < NumOfSteppers; i++)
|
||||
{
|
||||
pinMode(steppers[i].enPin, OUTPUT);
|
||||
pinMode(steppers[i].dirPin, OUTPUT);
|
||||
pinMode(steppers[i].stepPin, OUTPUT);
|
||||
}
|
||||
|
||||
#ifdef ESP32
|
||||
// Timer 1 - Equivalent to TIMER1_COMPA_vect
|
||||
timer1 = timerBegin(0, 80, true); // Timer 0, prescaler 80 (1us per tick)
|
||||
timerAttachInterrupt(timer1, &timer1ISR, true);
|
||||
timerAlarmWrite(timer1, 500, true); // 500us interval
|
||||
timerAlarmEnable(timer1);
|
||||
|
||||
// Timer 2 - Equivalent to TIMER3_COMPA_vect
|
||||
timer2 = timerBegin(1, 80, true); // Timer 1, prescaler 80 (1us per tick)
|
||||
timerAttachInterrupt(timer2, &timer2ISR, true);
|
||||
timerAlarmWrite(timer2, 500, true); // 500us interval
|
||||
timerAlarmEnable(timer2);
|
||||
|
||||
// Timer 3 - Equivalent to TIMER4_COMPA_vect
|
||||
timer3 = timerBegin(2, 80, true); // Timer 2, prescaler 80 (1us per tick)
|
||||
timerAttachInterrupt(timer3, &timer3ISR, true);
|
||||
timerAlarmWrite(timer3, 500, true); // 500us interval
|
||||
timerAlarmEnable(timer3);
|
||||
#else
|
||||
cli();
|
||||
// Timer 3 (16-bit)
|
||||
TCCR3A = 0;
|
||||
TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler
|
||||
OCR3A = 499; // Compare match value
|
||||
TIMSK3 |= (1 << OCIE3A); // Enable Timer1 Compare A Match Interrupt
|
||||
|
||||
// Timer 4 (16-bit)
|
||||
TCCR4A = 0;
|
||||
TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler
|
||||
OCR4A = 499; // Compare match value
|
||||
TIMSK4 |= (1 << OCIE4A); // Enable Timer3 Compare A Match Interrupt
|
||||
|
||||
// Timer 5 (16-bit)
|
||||
TCCR5A = 0;
|
||||
TCCR5B = (1 << WGM42) | (1 << CS50); // CTC mode, no prescaler
|
||||
OCR5A = 499; // Compare match value
|
||||
TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt
|
||||
sei();
|
||||
#endif
|
||||
}
|
||||
|
||||
StepperController::~StepperController()
|
||||
{
|
||||
// free(Steppers); // Meh, let's go for a memory leak. ez
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue