Simple version now working just as good as that complex
This commit is contained in:
parent
334ea6feaa
commit
a9d468d87c
|
|
@ -14,14 +14,23 @@ struct CharMapping {
|
|||
int value;
|
||||
};
|
||||
|
||||
// [X, Y, Head] TODO:
|
||||
#define NUM_MOTORS 3
|
||||
static const uint8_t STEP_PINS[NUM_MOTORS] = {54, 60, 36};
|
||||
static const uint8_t DIR_PINS[NUM_MOTORS] = {55, 61, 34};
|
||||
static const uint8_t EN_PINS[NUM_MOTORS] = {38, 56, 30};
|
||||
bool motorsEnabled[NUM_MOTORS] = {false, false, false};
|
||||
CharMapping mappings[] = {{'x', 0}, {'y', 1}, {'e', 2}};
|
||||
uint8_t selectedIndex = 1;
|
||||
struct Stepper {
|
||||
uint8_t enPin;
|
||||
uint8_t dirPin;
|
||||
uint8_t stepPin;
|
||||
bool timerEnabled;
|
||||
bool motorEnabled;
|
||||
char axis;
|
||||
};
|
||||
|
||||
|
||||
// [X, Y, Head]
|
||||
#define NUM_STEPPERS 3
|
||||
Stepper steppers[NUM_STEPPERS] = {
|
||||
{38, 55, 54, false, false, 'x'},
|
||||
{56, 61, 60, false, false, 'y'},
|
||||
{30, 34, 36, false, false, 'e'},
|
||||
};
|
||||
|
||||
// Critical range for detecting joystick movement
|
||||
static const int JOY_CRITICAL_POINT = 25;
|
||||
|
|
@ -63,108 +72,72 @@ void IRAM_ATTR timer3ISR()
|
|||
|
||||
// Timers for stepping the motors
|
||||
ISR(TIMER3_COMPA_vect) {
|
||||
if (motorsEnabled[0]) {
|
||||
digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
|
||||
if (steppers[0].timerEnabled) {
|
||||
digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin));
|
||||
}
|
||||
}
|
||||
ISR(TIMER4_COMPA_vect) {
|
||||
if (motorsEnabled[1]) {
|
||||
digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
|
||||
if (steppers[1].timerEnabled) {
|
||||
digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin));
|
||||
}
|
||||
}
|
||||
ISR(TIMER5_COMPA_vect) {
|
||||
if (motorsEnabled[2]) {
|
||||
digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
|
||||
if (steppers[2].timerEnabled) {
|
||||
digitalWrite(steppers[2].stepPin, !digitalRead(steppers[2].stepPin));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
int charToMotorIndex(char c) {
|
||||
for (int i = 0; i < NUM_MOTORS; i++) {
|
||||
if (mappings[i].character == c) {
|
||||
return mappings[i].value;
|
||||
}
|
||||
}
|
||||
return -1; // Not found
|
||||
}
|
||||
|
||||
void updateInput() {
|
||||
int joystickValueRaw = analogRead(JOYSTICK_PIN);
|
||||
int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
|
||||
bool leftMovement = (joystickValue > (100 - JOY_CRITICAL_POINT));
|
||||
bool rightMovement = (joystickValue < JOY_CRITICAL_POINT);
|
||||
motorsEnabled[selectedIndex] = leftMovement || rightMovement;
|
||||
|
||||
digitalWrite(EN_PINS[selectedIndex], !motorsEnabled[selectedIndex]); // Inverted for some reason
|
||||
digitalWrite(DIR_PINS[selectedIndex], leftMovement);
|
||||
|
||||
// // Debug
|
||||
// Serial.print("Raw: "); Serial.print(joystickValueRaw);
|
||||
// Serial.print(" | Mapped: "); Serial.print(joystickValue);
|
||||
// Serial.print(" | Left: "); Serial.print(leftMovement);
|
||||
// Serial.print(" | Right: "); Serial.print(rightMovement);
|
||||
// Serial.print(" | Motor: "); Serial.println(motorsEnabled[selectedIndex] ? "ON" : "OFF");
|
||||
}
|
||||
|
||||
void ExecuteCommand(void *params){
|
||||
ZCommand receivedValue = *((ZCommand *)params);
|
||||
}
|
||||
|
||||
int getStepperIndex(const char axis){
|
||||
for (size_t i = 0; i < NUM_STEPPERS; i++)
|
||||
{
|
||||
if (steppers[i].axis == axis) {
|
||||
Serial.print("Found motor: ");
|
||||
Serial.println(i);
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void MoveMotor(void *params) {
|
||||
MotorMoveParams *motorParams = ((MotorMoveParams *) params);
|
||||
char motorChar = motorParams->motorChar;
|
||||
float moveTimeMs = motorParams->moveTimeMs;
|
||||
|
||||
int index = charToMotorIndex(motorChar);
|
||||
int isBackwards = signbit(moveTimeMs); // Negative means move backwards, positive means move forward.
|
||||
|
||||
int index = getStepperIndex(motorChar);
|
||||
Stepper *currentStepper = &steppers[index];
|
||||
|
||||
// Enable motor and set direction
|
||||
motorsEnabled[index] = true;
|
||||
digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
|
||||
digitalWrite(DIR_PINS[index], isBackwards);
|
||||
Serial.println("Start motor.");
|
||||
currentStepper->motorEnabled = true;
|
||||
currentStepper->timerEnabled = true;
|
||||
digitalWrite(currentStepper->enPin, !(currentStepper->motorEnabled)); // Inverted for some reason
|
||||
digitalWrite(currentStepper->dirPin, isBackwards);
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(moveTimeMs));
|
||||
|
||||
// Stop motor
|
||||
motorsEnabled[index] = false;
|
||||
digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
|
||||
Serial.println("Stop motor.");
|
||||
currentStepper->motorEnabled = false;
|
||||
currentStepper->timerEnabled = false;
|
||||
digitalWrite(currentStepper->enPin, !(currentStepper->motorEnabled)); // Inverted for some reason
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
|
||||
void MotorControlTask(void *params) {
|
||||
while (true) {
|
||||
updateInput();
|
||||
|
||||
// Check if there is serial data available
|
||||
while (Serial.available() > 0) {
|
||||
String input = Serial.readStringUntil('\n'); // Read input until newline
|
||||
input.trim(); // Remove any whitespace
|
||||
|
||||
// Check if the input matches the expected format ":0", ":1", or ":2"
|
||||
if (input.length() == 2 && input[0] == ':') {
|
||||
char indexChar = input[1];
|
||||
if (indexChar >= '0' && indexChar <= '2') {
|
||||
selectedIndex = indexChar - '0'; // Convert character to integer
|
||||
Serial.print("Selected index set to: ");
|
||||
Serial.println(selectedIndex);
|
||||
} else {
|
||||
Serial.println("Invalid index! Use :0, :1, or :2");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(50)); // Poll every 50ms
|
||||
}
|
||||
}
|
||||
|
||||
// Configures Timer1 for compare match interrupts
|
||||
void setupMotor() {
|
||||
for (size_t i = 0; i < NUM_MOTORS; i++)
|
||||
for (size_t i = 0; i < NUM_STEPPERS; i++)
|
||||
{
|
||||
pinMode(STEP_PINS[i], OUTPUT);
|
||||
pinMode(DIR_PINS[i], OUTPUT);
|
||||
pinMode(EN_PINS[i], OUTPUT);
|
||||
pinMode(steppers[i].enPin, OUTPUT);
|
||||
pinMode(steppers[i].dirPin, OUTPUT);
|
||||
pinMode(steppers[i].stepPin, OUTPUT);
|
||||
}
|
||||
|
||||
#ifdef ESP32
|
||||
|
|
|
|||
Loading…
Reference in New Issue