diff --git a/examples/AccelTest/AccelTest.ino b/examples/AccelTest/AccelTest.ino new file mode 100644 index 0000000..c546862 --- /dev/null +++ b/examples/AccelTest/AccelTest.ino @@ -0,0 +1,54 @@ +/* + * Demo / test acceleration parameters + * + * Copyright (C)2015-2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include +#include "BasicStepperDriver.h" + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 + +// Since microstepping is set externally, make sure this matches the selected mode +// 1=full step, 2=half step etc. +#define MICROSTEPS 16 + +#define DIR 5 +#define STEP 9 + +// 2-wire basic config, microstepping is hardwired on the driver +BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP, 10); + +void setup() { + Serial.begin(115200); + + stepper.begin(120, MICROSTEPS); + + /* + * LINEAR_SPEED profile needs the acceleration and deceleration values + * in full steps / s^2. + */ + stepper.setSpeedProfile(LINEAR_SPEED, 1000, 1000); + + Serial.println("START"); + stepper.startRotate(360); +} + +void loop() { + static int step = 0; + unsigned wait_time = stepper.nextAction(); + if (wait_time){ + Serial.print(" step="); Serial.print(step++/2); + Serial.print(" dt="); Serial.print(wait_time); + Serial.print(" rpm="); Serial.print(stepper.getCurrentRPM()); + Serial.println(); + microWaitUntil(micros() + wait_time); + } else { + stepper.disable(); + Serial.println("END"); + delay(3600000); + } +} diff --git a/examples/BasicStepperDriver/BasicStepperDriver.ino b/examples/BasicStepperDriver/BasicStepperDriver.ino index 3410ec8..ce41c7c 100644 --- a/examples/BasicStepperDriver/BasicStepperDriver.ino +++ b/examples/BasicStepperDriver/BasicStepperDriver.ino @@ -35,8 +35,11 @@ void setup() { * Set target motor RPM. * These motors can do up to about 200rpm. * Too high will result in a high pitched whine and the motor does not move. + * + * Also tell the driver the microstep level we selected. + * If mismatched, the motor will move at a different RPM than chosen. */ - stepper.setRPM(120); + stepper.begin(120, MICROSTEPS); } void loop() { @@ -44,12 +47,6 @@ void loop() { // energize coils - the motor will hold position // stepper.enable(); - /* - * Tell the driver the microstep level we selected. - * If mismatched, the motor will move at a different RPM than chosen. - */ - stepper.setMicrostep(MICROSTEPS); - /* * Moving motor one full revolution using the degree notation */ diff --git a/examples/ClockStepper/ClockStepper.ino b/examples/ClockStepper/ClockStepper.ino index e9b3271..aee4949 100644 --- a/examples/ClockStepper/ClockStepper.ino +++ b/examples/ClockStepper/ClockStepper.ino @@ -43,10 +43,9 @@ DRV8834 stepper(MOTOR_STEPS, DIR, STEP, M0, M1); void setup() { /* - * Set target motor RPM. + * Set target motor RPM=1 and microstepping=1 */ - stepper.setRPM(1); - stepper.setMicrostep(1); // make sure we are in full speed mode + stepper.begin(1, 1); } void loop() { diff --git a/examples/MicroStepping/MicroStepping.ino b/examples/MicroStepping/MicroStepping.ino index ced0998..8e6d539 100644 --- a/examples/MicroStepping/MicroStepping.ino +++ b/examples/MicroStepping/MicroStepping.ino @@ -49,7 +49,7 @@ void setup() { * These motors can do up to about 200rpm. * Too high will result in a high pitched whine and the motor does not move. */ - stepper.setRPM(120); + stepper.begin(120); } void loop() { diff --git a/examples/MultiAxis/MultiAxis.ino b/examples/MultiAxis/MultiAxis.ino new file mode 100644 index 0000000..4852906 --- /dev/null +++ b/examples/MultiAxis/MultiAxis.ino @@ -0,0 +1,53 @@ +/* + * Multi-motor control + * + * Move two or three motors at the same time. + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include +#include "BasicStepperDriver.h" +#include "SyncDriver.h" + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 + +// X motor +#define DIR_X 5 +#define STEP_X 9 + +// Y motor +#define DIR_Y 8 +#define STEP_Y 6 + +// If microstepping is set externally, make sure this matches the selected mode +// 1=full step, 2=half step etc. +#define MICROSTEPS 32 + +// 2-wire basic config, microstepping is hardwired on the driver +// Other drivers can be mixed and matched but must be configured individually +BasicStepperDriver stepperX(MOTOR_STEPS, DIR_X, STEP_X); +BasicStepperDriver stepperY(MOTOR_STEPS, DIR_Y, STEP_Y); + +SyncDriver controller(stepperX, stepperY); + +void setup() { + /* + * Set target motors RPM. + */ + stepperX.begin(30, MICROSTEPS); + stepperY.begin(90, MICROSTEPS); +} + +void loop() { + + controller.rotate(90*5, 60*15); + delay(1000); + controller.rotate(-90*5, -30*15); + delay(1000); + controller.rotate(0, -30*15); + delay(30000); +} diff --git a/keywords.txt b/keywords.txt index e5cf898..55b78f9 100644 --- a/keywords.txt +++ b/keywords.txt @@ -5,8 +5,20 @@ DRV8834 KEYWORD1 DRV8824 KEYWORD1 DRV8825 KEYWORD1 A4988 KEYWORD1 +MultiDriver KEYWORD1 +SyncDriver KEYWORD1 setMicrostep KEYWORD2 +setSpeedProfile KEYWORD2 move KEYWORD2 rotate KEYWORD2 setRPM KEYWORD2 +getRPM KEYWORD2 +enable KEYWORD2 +disable KEYWORD2 +startMove KEYWORD2 +startRotate KEYWORD2 +nextAction KEYWORD2 + +CONSTANT_SPEED LITERAL1 +LINEAR_SPEED LITERAL1 diff --git a/library.properties b/library.properties index 192cc04..a7145b1 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=StepperDriver -version=1.0.6 +version=1.1.0 author=Laurentiu Badea maintainer=Laurentiu Badea sentence=A4988, DRV8825 and generic two-pin stepper motor driver library. -paragraph=Control steppers via a driver board providing STEP+DIR. Microstepping is supported. Supported drivers are A4988, DRV8824, DRV8825, DRV8834. +paragraph=Control steppers via a driver board providing STEP+DIR. Microstepping is supported. Acceleration is supported. Supported drivers are A4988, DRV8824, DRV8825, DRV8834. category=Device Control url=https://github.com/laurb9/StepperDriver architectures=* diff --git a/src/A4988.cpp b/src/A4988.cpp index b9ba490..08b0ad1 100644 --- a/src/A4988.cpp +++ b/src/A4988.cpp @@ -19,11 +19,11 @@ const uint8_t A4988::MS_TABLE[] = {0b000, 0b001, 0b010, 0b011, 0b111}; * Basic connection: only DIR, STEP are connected. * Microstepping controls should be hardwired. */ -A4988::A4988(int steps, int dir_pin, int step_pin) +A4988::A4988(short steps, short dir_pin, short step_pin) :BasicStepperDriver(steps, dir_pin, step_pin) {} -A4988::A4988(int steps, int dir_pin, int step_pin, int enable_pin) +A4988::A4988(short steps, short dir_pin, short step_pin, short enable_pin) :BasicStepperDriver(steps, dir_pin, step_pin, enable_pin) {} @@ -31,18 +31,18 @@ A4988::A4988(int steps, int dir_pin, int step_pin, int enable_pin) * Fully wired. * All the necessary control pins for A4988 are connected. */ -A4988::A4988(int steps, int dir_pin, int step_pin, int ms1_pin, int ms2_pin, int ms3_pin) +A4988::A4988(short steps, short dir_pin, short step_pin, short ms1_pin, short ms2_pin, short ms3_pin) :BasicStepperDriver(steps, dir_pin, step_pin), ms1_pin(ms1_pin), ms2_pin(ms2_pin), ms3_pin(ms3_pin) {} -A4988::A4988(int steps, int dir_pin, int step_pin, int enable_pin, int ms1_pin, int ms2_pin, int ms3_pin) +A4988::A4988(short steps, short dir_pin, short step_pin, short enable_pin, short ms1_pin, short ms2_pin, short ms3_pin) :BasicStepperDriver(steps, dir_pin, step_pin, enable_pin), ms1_pin(ms1_pin), ms2_pin(ms2_pin), ms3_pin(ms3_pin) {} -void A4988::init(void){ - BasicStepperDriver::init(); +void A4988::begin(short rpm, short microsteps){ + BasicStepperDriver::begin(rpm, microsteps); if (!IS_CONNECTED(ms1_pin) || !IS_CONNECTED(ms2_pin) || !IS_CONNECTED(ms3_pin)){ return; @@ -58,17 +58,17 @@ void A4988::init(void){ * Allowed ranges for A4988 are 1:1 to 1:16 * If the control pins are not connected, we recalculate the timing only */ -unsigned A4988::setMicrostep(unsigned microsteps){ +short A4988::setMicrostep(short microsteps){ BasicStepperDriver::setMicrostep(microsteps); if (!IS_CONNECTED(ms1_pin) || !IS_CONNECTED(ms2_pin) || !IS_CONNECTED(ms3_pin)){ return this->microsteps; } - const uint8_t* ms_table = this->getMicrostepTable(); - size_t ms_table_size = this->getMicrostepTableSize(); + const uint8_t* ms_table = getMicrostepTable(); + size_t ms_table_size = getMicrostepTableSize(); - int i = 0; + unsigned short i = 0; while (i < ms_table_size){ if (this->microsteps & (1<rpm = rpm; + setMicrostep(microsteps); enable(); } - -void BasicStepperDriver::calcStepPulse(void){ - step_pulse = STEP_PULSE(rpm, motor_steps, microsteps); -} - /* * Set target motor RPM (1-200 is a reasonable range) */ -void BasicStepperDriver::setRPM(unsigned rpm){ +void BasicStepperDriver::setRPM(short rpm){ + if (this->rpm == 0){ // begin() has not been called (old 1.0 code) + begin(rpm, microsteps); + } this->rpm = rpm; - calcStepPulse(); } /* * Set stepping mode (1:microsteps) * Allowed ranges for BasicStepperDriver are 1:1 to 1:128 */ -unsigned BasicStepperDriver::setMicrostep(unsigned microsteps){ - for (unsigned ms=1; ms <= this->getMaxMicrostep(); ms<<=1){ +short BasicStepperDriver::setMicrostep(short microsteps){ + for (short ms=1; ms <= getMaxMicrostep(); ms<<=1){ if (microsteps == ms){ this->microsteps = microsteps; break; } } - calcStepPulse(); return this->microsteps; } /* - * DIR: forward HIGH, reverse LOW + * Set speed profile - CONSTANT_SPEED, LINEAR_SPEED (accelerated) + * accel and decel are given in [full steps/s^2] */ -void BasicStepperDriver::setDirection(int direction){ - digitalWrite(dir_pin, (direction<0) ? LOW : HIGH); +void BasicStepperDriver::setSpeedProfile(Mode mode, short accel, short decel){ + this->mode = mode; + this->accel = accel; + this->decel = decel; } /* @@ -83,32 +85,18 @@ void BasicStepperDriver::setDirection(int direction){ * positive to move forward, negative to reverse */ void BasicStepperDriver::move(long steps){ - if (steps >= 0){ - setDirection(1); - } else { - setDirection(-1); - steps = -steps; - } - /* - * We currently try to do a 50% duty cycle so it's easy to see. - * Other option is step_high_min, pulse_duration-step_high_min. - */ - unsigned long pulse_duration = step_pulse/2; - while (steps--){ - digitalWrite(step_pin, HIGH); - unsigned long next_edge = micros() + pulse_duration; - microWaitUntil(next_edge); - digitalWrite(step_pin, LOW); - microWaitUntil(next_edge + pulse_duration); - } + long next_event; + startMove(steps); + do { + next_event = nextAction(); + microWaitUntil(micros() + next_event); + } while (next_event); } - /* * Move the motor a given number of degrees (1-360) */ void BasicStepperDriver::rotate(long deg){ - long steps = deg * motor_steps * (long)microsteps / 360; - move(steps); + move(calcStepsForRotation(deg)); } /* * Move the motor with sub-degree precision. @@ -116,8 +104,142 @@ void BasicStepperDriver::rotate(long deg){ * due to inclusion of float support. */ void BasicStepperDriver::rotate(double deg){ - long steps = deg * motor_steps * microsteps / 360; - move(steps); + move(calcStepsForRotation(deg)); +} + +/* + * Set up a new move (calculate and save the parameters) + */ +void BasicStepperDriver::startMove(long steps){ + long speed; + dir_state = (steps >= 0) ? HIGH : LOW; + step_state = LOW; + steps_remaining = abs(steps); + step_count = 0; + switch (mode){ + case LINEAR_SPEED: + // speed is in [steps/s] + speed = rpm * motor_steps / 60; + // how many steps from 0 to target rpm + steps_to_cruise = speed * speed * microsteps / (2 * accel); + // how many steps from 0 til we need to begin slowing down + steps_to_brake = steps_remaining * decel / (accel + decel); + if (steps_to_cruise < steps_to_brake){ + // will reach max speed before needing to brake + steps_to_brake = steps_to_cruise * accel / decel; + } else { + // cannot reach max speed, will need to brake early + steps_to_cruise = steps_to_brake; + steps_to_brake = steps_remaining - steps_to_cruise; + } + // Initial pulse (c0) including error correction factor 0.676 [us] + step_pulse = (1e+6)*0.676*sqrt(2.0f/(accel*microsteps)); + break; + case CONSTANT_SPEED: + default: + step_pulse = STEP_PULSE(rpm, motor_steps, microsteps); + } +} +/* + * Return calculated time to complete the given move + */ +long BasicStepperDriver::getTimeForMove(long steps){ + long t; + switch (mode){ + case LINEAR_SPEED: + startMove(steps); + t = sqrt(2 * steps_to_cruise / accel) + + (steps_remaining - steps_to_cruise - steps_to_brake) * STEP_PULSE(rpm, motor_steps, microsteps) + + sqrt(2 * steps_to_brake / decel); + break; + case CONSTANT_SPEED: + default: + t = STEP_PULSE(rpm, motor_steps, microsteps); + } + return t; +} +/* + * Move the motor an integer number of degrees (360 = full rotation) + * This has poor precision for small amounts, since step is usually 1.8deg + */ +void BasicStepperDriver::startRotate(long deg){ + startMove(calcStepsForRotation(deg)); +} +/* + * Move the motor with sub-degree precision. + * Note that calling this function will increase program size substantially + * due to inclusion of float support. + */ +void BasicStepperDriver::startRotate(double deg){ + startMove(calcStepsForRotation(deg)); +} + +/* + * calculate the interval til the next pulse + */ +void BasicStepperDriver::calcStepPulse(void){ + // remainder to be fed into successive steps to increase accuracy (Atmel DOC8017) + static long rest; + + if (steps_remaining <= 0){ // this should not happen, but avoids strange calculations + return; + } + + steps_remaining--; + step_count++; + + // if constant speed + if (mode == LINEAR_SPEED){ + if (step_count <= steps_to_cruise){ + if (step_count == 1){ // first step, initialize rest + rest = 0; + } + /* + * accelerating + */ + step_pulse = step_pulse - (2*step_pulse+rest)/(4*step_count+1); + rest = (step_count < steps_to_cruise) ? (2*step_pulse+rest) % (4*step_count+1) : 0; + } else if (steps_remaining > steps_to_brake){ + /* + * cruising (no speed changes) + */ + } else { + /* + * decelerating + */ + step_pulse = step_pulse - (2*step_pulse+rest)/(-4*steps_remaining+1); + rest = (2*step_pulse+rest) % (-4*steps_remaining+1); + } + } +} +/* + * Toggle step and return time until next change is needed (micros) + */ +long BasicStepperDriver::nextAction(void){ + if (steps_remaining > 0){ + /* + * DIR pin is sampled on rising STEP edge, so it is set first + */ + digitalWrite(dir_pin, dir_state); + if (step_state == LOW){ + step_state = HIGH; + } else { + step_state = LOW; + } + digitalWrite(step_pin, step_state); + unsigned m = micros(); + if (step_state == LOW){ + calcStepPulse(); + } + m = micros() - m; + /* + * We currently try to do a 50% duty cycle so it's easy to see. + * Other option is step_high_min, pulse_duration-step_high_min. + */ + return (step_state == LOW) ? step_pulse-step_high_min-m : step_high_min; + } else { + return 0; // end of move + } } /* @@ -135,6 +257,6 @@ void BasicStepperDriver::disable(void){ } } -unsigned BasicStepperDriver::getMaxMicrostep(){ +short BasicStepperDriver::getMaxMicrostep(){ return BasicStepperDriver::MAX_MICROSTEP; } diff --git a/src/BasicStepperDriver.h b/src/BasicStepperDriver.h index 8f9c6bd..87f8ed9 100644 --- a/src/BasicStepperDriver.h +++ b/src/BasicStepperDriver.h @@ -2,7 +2,7 @@ * Generic Stepper Motor Driver Driver * Indexer mode only. * - * Copyright (C)2015 Laurentiu Badea + * Copyright (C)2015-2017 Laurentiu Badea * * This file may be redistributed under the terms of the MIT license. * A copy of this license has been included with this distribution in the file LICENSE. @@ -10,12 +10,14 @@ #ifndef STEPPER_DRIVER_BASE_H #define STEPPER_DRIVER_BASE_H #include -#include "BasicStepperDriver.h" // used internally by the library to mark unconnected pins #define PIN_UNCONNECTED -1 #define IS_CONNECTED(pin) (pin != PIN_UNCONNECTED) +enum Direction {DIR_FORWARD, DIR_REVERSE}; +enum Mode {CONSTANT_SPEED, LINEAR_SPEED}; + /* * calculate the step pulse in microseconds for a given rpm value. * 60[s/min] * 1000000[us/s] / microsteps / steps / rpm @@ -34,22 +36,23 @@ inline void microWaitUntil(unsigned long target_micros){ */ class BasicStepperDriver { protected: - int motor_steps; - int rpm = 60; - int dir_pin; - int step_pin; - int enable_pin = PIN_UNCONNECTED; - - // current microstep level, must be < getMaxMicrostep() - // for 1:16 microsteps is 16 - unsigned microsteps = 1; - // step pulse duration (microseconds), depends on rpm and microstep level - unsigned long step_pulse; - - void setDirection(int direction); - void init(void); - void calcStepPulse(void); + /* + * Motor Configuration + */ + short motor_steps; // motor steps per revolution (usually 200) + short accel = 1000; // maximum acceleration [steps/s^2] + short decel = 1000; // maximum deceleration [steps/s^2] + /* + * Driver Configuration + */ + short dir_pin; + short step_pin; + short enable_pin = PIN_UNCONNECTED; + // Get max microsteps supported by the device + virtual short getMaxMicrostep(); + // current microstep level (1,2,4,8,...), must be < getMaxMicrostep() + short microsteps = 1; // tWH(STEP) pulse duration, STEP high, min value (us) static const int step_high_min = 1; // tWL(STEP) pulse duration, STEP low, min value (us) @@ -57,24 +60,59 @@ class BasicStepperDriver { // tWAKE wakeup time, nSLEEP inactive to STEP (us) static const int wakeup_time = 0; - // Get max microsteps supported by the device - virtual unsigned getMaxMicrostep(); + short rpm = 0; + + /* + * Movement state + */ + Mode mode = CONSTANT_SPEED; + long step_count; // current position + long steps_remaining; // to complete the current move (absolute value) + long steps_to_cruise; // steps to reach cruising (max) rpm + long steps_to_brake; // steps needed to come to a full stop + long step_pulse; // step pulse duration (microseconds) + + // DIR pin state + short dir_state; + // STEP pin state (HIGH / LOW) + short step_state = LOW; + + void calcStepPulse(void); private: // microstep range (1, 16, 32 etc) - static const unsigned MAX_MICROSTEP = 128; + static const short MAX_MICROSTEP = 128; public: /* * Basic connection: DIR, STEP are connected. */ - BasicStepperDriver(int steps, int dir_pin, int step_pin); - BasicStepperDriver(int steps, int dir_pin, int step_pin, int enable_pin); + BasicStepperDriver(short steps, short dir_pin, short step_pin); + BasicStepperDriver(short steps, short dir_pin, short step_pin, short enable_pin); + /* + * Initialize pins, calculate timings etc + */ + void begin(short rpm=60, short microsteps=1); /* * Set current microstep level, 1=full speed, 32=fine microstepping * Returns new level or previous level if value out of range */ - unsigned setMicrostep(unsigned microsteps); + virtual short setMicrostep(short microsteps); + /* + * Set target motor RPM (1-200 is a reasonable range) + */ + void setRPM(short rpm); + short getRPM(void){ + return rpm; + }; + short getCurrentRPM(void){ + return (short)(60*1000000L / step_pulse / microsteps / motor_steps); + } + /* + * Set speed profile - CONSTANT_SPEED, LINEAR_SPEED (accelerated) + * accel and decel are given in [full steps/s^2] + */ + void setSpeedProfile(Mode mode, short accel=1000, short decel=1000); /* * Move the motor a given number of steps. * positive to move forward, negative to reverse @@ -91,14 +129,41 @@ class BasicStepperDriver { * Rotate using a float or double for increased movement precision. */ void rotate(double deg); - /* - * Set target motor RPM (1-200 is a reasonable range) - */ - void setRPM(unsigned rpm); /* * Turn off/on motor to allow the motor to be moved by hand/hold the position in place */ void enable(void); void disable(void); + /* + * Methods to allow external timing control. + * These should not be needed for normal use. + */ + /* + * Initiate a move (calculate and save the parameters) + */ + void startMove(long steps); + inline void startRotate(int deg){ + startRotate((long)deg); + }; + void startRotate(long deg); + void startRotate(double deg); + /* + * Toggle step and return time until next change is needed (micros) + */ + long nextAction(void); + + /* + * Return calculated time to complete the given move + */ + long getTimeForMove(long steps); + /* + * Calculate steps needed to rotate requested angle, given in degrees + */ + long calcStepsForRotation(long deg){ + return deg * motor_steps * (long)microsteps / 360; + } + long calcStepsForRotation(double deg){ + return deg * motor_steps * microsteps / 360; + } }; #endif // STEPPER_DRIVER_BASE_H diff --git a/src/DRV8825.cpp b/src/DRV8825.cpp index 359ce73..d0800b7 100644 --- a/src/DRV8825.cpp +++ b/src/DRV8825.cpp @@ -15,22 +15,22 @@ */ const uint8_t DRV8825::MS_TABLE[] = {0b000, 0b001, 0b010, 0b011, 0b100, 0b111}; -DRV8825::DRV8825(int steps, int dir_pin, int step_pin) +DRV8825::DRV8825(short steps, short dir_pin, short step_pin) :A4988(steps, dir_pin, step_pin) {} -DRV8825::DRV8825(int steps, int dir_pin, int step_pin, int enable_pin) +DRV8825::DRV8825(short steps, short dir_pin, short step_pin, short enable_pin) :A4988(steps, dir_pin, step_pin, enable_pin) {} /* * A4988-DRV8825 Compatibility map: MS1-MODE0, MS2-MODE1, MS3-MODE2 */ -DRV8825::DRV8825(int steps, int dir_pin, int step_pin, int mode0_pin, int mode1_pin, int mode2_pin) +DRV8825::DRV8825(short steps, short dir_pin, short step_pin, short mode0_pin, short mode1_pin, short mode2_pin) :A4988(steps, dir_pin, step_pin, mode0_pin, mode1_pin, mode2_pin) {} -DRV8825::DRV8825(int steps, int dir_pin, int step_pin, int enable_pin, int mode0_pin, int mode1_pin, int mode2_pin) +DRV8825::DRV8825(short steps, short dir_pin, short step_pin, short enable_pin, short mode0_pin, short mode1_pin, short mode2_pin) :A4988(steps, dir_pin, step_pin, enable_pin, mode0_pin, mode1_pin, mode2_pin) {} @@ -44,6 +44,6 @@ size_t DRV8825::getMicrostepTableSize() return sizeof(DRV8825::MS_TABLE); } -unsigned DRV8825::getMaxMicrostep(){ +short DRV8825::getMaxMicrostep(){ return DRV8825::MAX_MICROSTEP; } diff --git a/src/DRV8825.h b/src/DRV8825.h index fe0c0b7..68afe8f 100644 --- a/src/DRV8825.h +++ b/src/DRV8825.h @@ -28,16 +28,16 @@ class DRV8825 : public A4988 { size_t getMicrostepTableSize() override; // Get max microsteps supported by the device - unsigned getMaxMicrostep() override; + short getMaxMicrostep() override; private: // microstep range (1, 16, 32 etc) - static const unsigned MAX_MICROSTEP = 32; + static const short MAX_MICROSTEP = 32; public: - DRV8825(int steps, int dir_pin, int step_pin); - DRV8825(int steps, int dir_pin, int step_pin, int enable_pin); - DRV8825(int steps, int dir_pin, int step_pin, int mode0_pin, int mode1_pin, int mode2_pin); - DRV8825(int steps, int dir_pin, int step_pin, int enable_pin, int mode0_pin, int mode1_pin, int mode2_pin); + DRV8825(short steps, short dir_pin, short step_pin); + DRV8825(short steps, short dir_pin, short step_pin, short enable_pin); + DRV8825(short steps, short dir_pin, short step_pin, short mode0_pin, short mode1_pin, short mode2_pin); + DRV8825(short steps, short dir_pin, short step_pin, short enable_pin, short mode0_pin, short mode1_pin, short mode2_pin); }; #endif // DRV8825_H diff --git a/src/DRV8834.cpp b/src/DRV8834.cpp index 917fdeb..6a048c3 100644 --- a/src/DRV8834.cpp +++ b/src/DRV8834.cpp @@ -13,22 +13,22 @@ * Basic connection: only DIR, STEP are connected. * Microstepping controls should be hardwired. */ -DRV8834::DRV8834(int steps, int dir_pin, int step_pin) +DRV8834::DRV8834(short steps, short dir_pin, short step_pin) :BasicStepperDriver(steps, dir_pin, step_pin) {} -DRV8834::DRV8834(int steps, int dir_pin, int step_pin, int enable_pin) +DRV8834::DRV8834(short steps, short dir_pin, short step_pin, short enable_pin) :BasicStepperDriver(steps, dir_pin, step_pin, enable_pin) {} /* * Fully wired. All the necessary control pins for DRV8834 are connected. */ -DRV8834::DRV8834(int steps, int dir_pin, int step_pin, int m0_pin, int m1_pin) +DRV8834::DRV8834(short steps, short dir_pin, short step_pin, short m0_pin, short m1_pin) :BasicStepperDriver(steps, dir_pin, step_pin), m0_pin(m0_pin), m1_pin(m1_pin) {} -DRV8834::DRV8834(int steps, int dir_pin, int step_pin, int enable_pin, int m0_pin, int m1_pin) +DRV8834::DRV8834(short steps, short dir_pin, short step_pin, short enable_pin, short m0_pin, short m1_pin) :BasicStepperDriver(steps, dir_pin, step_pin, enable_pin), m0_pin(m0_pin), m1_pin(m1_pin) {} @@ -38,7 +38,7 @@ DRV8834::DRV8834(int steps, int dir_pin, int step_pin, int enable_pin, int m0_pi * If the control pins are not connected, we recalculate the timing only * */ -unsigned DRV8834::setMicrostep(unsigned microsteps){ +short DRV8834::setMicrostep(short microsteps){ BasicStepperDriver::setMicrostep(microsteps); if (!IS_CONNECTED(m0_pin) || !IS_CONNECTED(m1_pin)){ @@ -80,6 +80,6 @@ unsigned DRV8834::setMicrostep(unsigned microsteps){ return this->microsteps; } -unsigned DRV8834::getMaxMicrostep(){ +short DRV8834::getMaxMicrostep(){ return DRV8834::MAX_MICROSTEP; } diff --git a/src/DRV8834.h b/src/DRV8834.h index 2b4433d..7aeeec6 100644 --- a/src/DRV8834.h +++ b/src/DRV8834.h @@ -14,9 +14,8 @@ class DRV8834 : public BasicStepperDriver { protected: - int m0_pin = PIN_UNCONNECTED; - int m1_pin = PIN_UNCONNECTED; - void init(void); + short m0_pin = PIN_UNCONNECTED; + short m1_pin = PIN_UNCONNECTED; // tWH(STEP) pulse duration, STEP high, min value (1.9us) static const int step_high_min = 2; // tWL(STEP) pulse duration, STEP low, min value (1.9us) @@ -26,24 +25,24 @@ class DRV8834 : public BasicStepperDriver { // also 200ns between ENBL/DIR/Mx changes and STEP HIGH // Get max microsteps supported by the device - unsigned getMaxMicrostep() override; + short getMaxMicrostep() override; private: // microstep range (1, 16, 32 etc) - static const unsigned MAX_MICROSTEP = 32; + static const short MAX_MICROSTEP = 32; public: /* * Basic connection: only DIR, STEP are connected. * Microstepping controls should be hardwired. */ - DRV8834(int steps, int dir_pin, int step_pin); - DRV8834(int steps, int dir_pin, int step_pin, int enable_pin); + DRV8834(short steps, short dir_pin, short step_pin); + DRV8834(short steps, short dir_pin, short step_pin, short enable_pin); /* * Fully wired. All the necessary control pins for DRV8834 are connected. */ - DRV8834(int steps, int dir_pin, int step_pin, int m0_pin, int m1_pin); - DRV8834(int steps, int dir_pin, int step_pin, int enable_pin, int m0_pin, int m1_pin); - unsigned setMicrostep(unsigned microsteps); + DRV8834(short steps, short dir_pin, short step_pin, short m0_pin, short m1_pin); + DRV8834(short steps, short dir_pin, short step_pin, short enable_pin, short m0_pin, short m1_pin); + short setMicrostep(short microsteps) override; }; #endif // DRV8834_H diff --git a/src/MultiDriver.cpp b/src/MultiDriver.cpp new file mode 100644 index 0000000..5fc8917 --- /dev/null +++ b/src/MultiDriver.cpp @@ -0,0 +1,101 @@ +/* + * Multi-motor group driver + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "MultiDriver.h" + +#define FOREACH_MOTOR(action) for (short i=count-1; i >= 0; i--){action;} + +/* + * Initialize motor parameters + */ +void MultiDriver::startMove(long steps1, long steps2, long steps3){ + long steps[3] = {steps1, steps2, steps3}; + /* + * Initialize state for all active motors + */ + FOREACH_MOTOR( + if (steps[i]){ + motors[i]->startMove(steps[i]); + event_timers[i] = 0; + } else { + event_timers[i] = -1; + } + ); + ready = false; +} +/* + * Trigger next step action + */ +long MultiDriver::nextAction(void){ + // Trigger all the motors that need it (event timer = 0) + FOREACH_MOTOR( + if (event_timers[i] == 0){ + event_timers[i] = motors[i]->nextAction(); + } + ); + // Find the time when the next pulse needs to fire + // this is the smallest non-zero timer value from all active motors + long next_event = 0; + ready = true; + FOREACH_MOTOR( + if (event_timers[i] > 0){ + ready = false; + if (event_timers[i] < next_event || next_event == 0){ + next_event = event_timers[i]; + } + } + ); + // Reduce all event timers by the current left time so 0 marks next + FOREACH_MOTOR( + if (event_timers[i] > 0){ + event_timers[i] -= next_event; + } + ); + return (ready) ? 0 : next_event; +} +/* + * Move each motor the requested number of steps, in parallel + * positive to move forward, negative to reverse, 0 to remain still + */ +void MultiDriver::move(long steps1, long steps2, long steps3){ + unsigned long next_event; + startMove(steps1, steps2, steps3); + while (!ready){ + next_event = nextAction(); + // wait until the next event + microWaitUntil(micros() + next_event); + } +} + +#define CALC_STEPS(i, deg) ((motors[i] && deg) ? motors[i]->calcStepsForRotation(deg) : 0) +void MultiDriver::rotate(long deg1, long deg2, long deg3){ + move(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::rotate(double deg1, double deg2, double deg3){ + move(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::startRotate(long deg1, long deg2, long deg3){ + startMove(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::startRotate(double deg1, double deg2, double deg3){ + startMove(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::setMicrostep(unsigned microsteps){ + FOREACH_MOTOR(motors[i]->setMicrostep(microsteps)); +} + +void MultiDriver::enable(void){ + FOREACH_MOTOR(motors[i]->enable()); +} +void MultiDriver::disable(void){ + FOREACH_MOTOR(motors[i]->disable()); +} diff --git a/src/MultiDriver.h b/src/MultiDriver.h new file mode 100644 index 0000000..1dc9183 --- /dev/null +++ b/src/MultiDriver.h @@ -0,0 +1,89 @@ +/* + * Multi-motor group driver + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef MULTI_DRIVER_H +#define MULTI_DRIVER_H +#include +#include "BasicStepperDriver.h" + +#define MAX_MOTORS 3 // a reasonable but arbitrary limit +#define Motor BasicStepperDriver +/* + * Multi-motor group driver class. + */ +class MultiDriver { +protected: + /* + * Configuration + */ + unsigned short count; + Motor* const *motors; + /* + * Generic initializer, will be called by the others + */ + MultiDriver(const unsigned short count, Motor* const *motors) + :count(count), motors(motors) + {}; + + /* + * Movement state + */ + // ready to start a new move + bool ready = true; + // when next state change is due for each motor + long event_timers[MAX_MOTORS]; + +public: + /* + * Two-motor setup + */ + MultiDriver(Motor& motor1, Motor& motor2) + :MultiDriver(2, new Motor* const[2]{&motor1, &motor2}) + {}; + /* + * Three-motor setup (X, Y, Z for example) + */ + MultiDriver(Motor& motor1, Motor& motor2, Motor& motor3) + :MultiDriver(3, new Motor* const[3]{&motor1, &motor2, &motor3}) + {}; + /* + * Move the motors a given number of steps. + * positive to move forward, negative to reverse + */ + void move(long steps1, long steps2, long steps3=0); + void rotate(int deg1, int deg2, int deg3=0){ + rotate((long)deg1, (long)deg2, (long)deg3); + }; + void rotate(long deg1, long deg2, long deg3=0); + void rotate(double deg1, double deg2, double deg3=0); + + /* + * Motor movement with external control of timing + */ + virtual void startMove(long steps1, long steps2, long steps3=0); + void startRotate(int deg1, int deg2, int deg3=0){ + startRotate((long)deg1, (long)deg2, (long)deg3); + }; + void startRotate(long deg1, long deg2, long deg3=0); + void startRotate(double deg1, double deg2, double deg3=0); + /* + * Toggle step and return time until next change is needed (micros) + */ + virtual long nextAction(void); + + /* + * Set the same microstepping level on all motors + */ + void setMicrostep(unsigned microsteps); + /* + * Turn all motors on or off + */ + void enable(void); + void disable(void); +}; +#endif // MULTI_DRIVER_H diff --git a/src/SyncDriver.cpp b/src/SyncDriver.cpp new file mode 100644 index 0000000..8a50305 --- /dev/null +++ b/src/SyncDriver.cpp @@ -0,0 +1,69 @@ +/* + * Synchronous Multi-motor group driver + * All motors reach their target at the same time. + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "SyncDriver.h" + +#define FOREACH_MOTOR(action) for (short i=count-1; i >= 0; i--){action;} + +/* + * Initialize motor parameters + */ +void SyncDriver::startMove(long steps1, long steps2, long steps3){ + long steps[3] = {steps1, steps2, steps3}; + long timing[MAX_MOTORS]; + /* + * find which motor would take the longest to finish, + */ + long move_time = 0; + FOREACH_MOTOR( + long m = motors[i]->getTimeForMove(abs(steps[i])); + timing[i] = m; + if (m > move_time){ + move_time = m; + } + ); + /* + * Stretch timing for all others by adjusting rpm proportionally + */ + if (move_time){ + FOREACH_MOTOR( + if (steps[i]){ + rpms[i] = motors[i]->getRPM(); + motors[i]->setRPM(rpms[i] * timing[i] / move_time); + } else { + rpms[i] = 0; + } + ); + } + /* + * Initialize state for all active motors + */ + FOREACH_MOTOR( + if (steps[i]){ + motors[i]->startMove(steps[i]); + }; + event_timers[i] = 0; + ); + ready = false; +} + +long SyncDriver::nextAction(void){ + long next_event = MultiDriver::nextAction(); + if (!next_event){ + /* + * Restore original rpm settings + */ + FOREACH_MOTOR( + if (rpms[i]){ + motors[i]->setRPM(rpms[i]); + } + ); + } + return next_event; +} diff --git a/src/SyncDriver.h b/src/SyncDriver.h new file mode 100644 index 0000000..c8421d3 --- /dev/null +++ b/src/SyncDriver.h @@ -0,0 +1,45 @@ +/* + * Synchronous Multi-motor group driver + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef SYNC_DRIVER_H +#define SYNC_DRIVER_H +#include +#include "MultiDriver.h" + +/* + * Synchronous Multi-motor group driver class. + * This driver sets up timing so all motors reach their target at the same time. + */ +class SyncDriver : public MultiDriver { +protected: + unsigned short rpms[MAX_MOTORS]; + /* + * Generic initializer, will be called by the others + */ + SyncDriver(const unsigned short count, Motor* const *motors) + :MultiDriver(count, motors) + {}; + +public: + /* + * Two-motor setup + */ + SyncDriver(Motor& motor1, Motor& motor2) + :SyncDriver(2, new Motor* const[2]{&motor1, &motor2}) + {}; + /* + * Three-motor setup (X, Y, Z for example) + */ + SyncDriver(Motor& motor1, Motor& motor2, Motor& motor3) + :SyncDriver(3, new Motor* const[3]{&motor1, &motor2, &motor3}) + {}; + + void startMove(long steps1, long steps2, long steps3=0) override; + long nextAction(void) override; +}; +#endif // SYNC_DRIVER_H