diff --git a/Lib/stepperQ/20171002_205103.jpg b/Lib/stepperQ/20171002_205103.jpg new file mode 100644 index 0000000..5e5d5c4 Binary files /dev/null and b/Lib/stepperQ/20171002_205103.jpg differ diff --git a/Lib/stepperQ/Examples/Blocking/Blocking.ino b/Lib/stepperQ/Examples/Blocking/Blocking.ino new file mode 100644 index 0000000..03c347f --- /dev/null +++ b/Lib/stepperQ/Examples/Blocking/Blocking.ino @@ -0,0 +1,39 @@ + +// Copyright (C) 2015 Alexander Chestnov + +#include + +int c = 0; + + + +int dir_pin = 7; +int step_pin = 6 ; + +void setup() { + + stepperq.init(dir_pin,step_pin); + stepperq.setMaxSpeed(200); + stepperq.setAcceleration(9900); + +} +void loop () { + + if ( stepperq.distanceToGo() == 0 ) { + + if (c%2 == 0) + { + stepperq.moveTo(400); + stepperq.start(); + } + else { + stepperq.moveTo(0); + stepperq.start(); + } + + c++; + } + + delay(100); + +} diff --git a/Lib/stepperQ/Examples/ChangeSpeed/ChangeSpeed.ino b/Lib/stepperQ/Examples/ChangeSpeed/ChangeSpeed.ino new file mode 100644 index 0000000..24ebe52 --- /dev/null +++ b/Lib/stepperQ/Examples/ChangeSpeed/ChangeSpeed.ino @@ -0,0 +1,42 @@ + +// Copyright (C) 2015 Alexander Chestnov + +#include + +int c = 0; + +int dir_pin = 7; +int step_pin = 6 ; + +void setup() { + + stepperq.init(dir_pin,step_pin); + stepperq.setMaxSpeed(200); + stepperq.setAcceleration(9900); + stepperq.moveTo(8000); + + stepperq.start(); + +} +void loop () { + + if ( stepperq.distanceToGo() <4000 ) { + stepperq.move(8000); + } + + return ; + + if (c%2 == 0) + { + stepperq.setMaxSpeed(200); + + } + else { + + stepperq.setMaxSpeed(300); + } + + c++; + delay(1000); + +} diff --git a/Lib/stepperQ/Examples/ProportionalControl/ProportionalControl.ino b/Lib/stepperQ/Examples/ProportionalControl/ProportionalControl.ino new file mode 100644 index 0000000..3bb9e05 --- /dev/null +++ b/Lib/stepperQ/Examples/ProportionalControl/ProportionalControl.ino @@ -0,0 +1,45 @@ +// ProportionalControl.pde +// -*- mode: C++ -*- +// +// Make a single stepper follow the analog value read from a pot or whatever +// The stepper will move at a constant speed to each newly set posiiton, +// depending on the value of the pot. +// +// Copyright (C) 2015 Alexander Chestnov + +#include + + + +// Define a stepper and the pins it will use + +int dir_pin = 7; +int step_pin = 6 ; + + +// This defines the analog input pin for reading the control voltage +// Tested with a 10k linear pot between 5v and GND +#define ANALOG_IN A0 + +void setup() +{ + stepperq.init(dir_pin,step_pin); + stepperq.setAcceleration(9900); + stepperq.setMaxSpeed(1000); + stepperq.moveTo(8000); + + stepperq.start(); +} + +void loop() +{ + if ( stepperq.distanceToGo() <4000 ) { + stepperq.move(8000); + + } + + // Read new Speed + int analog_in = analogRead(ANALOG_IN); + stepperq.setMaxSpeed(analog_in); + delay(50); +} diff --git a/Lib/stepperQ/Examples/ProportionalSpeedControl/ProportionalSpeedControl.ino b/Lib/stepperQ/Examples/ProportionalSpeedControl/ProportionalSpeedControl.ino new file mode 100644 index 0000000..1d904ee --- /dev/null +++ b/Lib/stepperQ/Examples/ProportionalSpeedControl/ProportionalSpeedControl.ino @@ -0,0 +1,46 @@ +// ProportionalControl.pde +// -*- mode: C++ -*- +// +// Make a single stepper follow the analog value read from a pot or whatever +// The stepper will move at a constant speed to each newly set posiiton, +// depending on the value of the pot. +// +// Copyright (C) 2012 Mike McCauley +// $Id: ProportionalControl.pde,v 1.1 2011/01/05 01:51:01 mikem Exp mikem $ + +#include + + + +// Define a stepper and the pins it will use + +int dir_pin = 7; +int step_pin = 6 ; + + +// This defines the analog input pin for reading the control voltage +// Tested with a 10k linear pot between 5v and GND +#define ANALOG_IN A0 + +void setup() +{ + stepperq.init(dir_pin,step_pin); + stepperq.setAcceleration(9900); + stepperq.setMaxSpeed(1000); + stepperq.moveTo(8000); + + stepperq.start(); +} + +void loop() +{ + if ( stepperq.distanceToGo() <4000 ) { + stepperq.move(8000); + + } + + // Read new Speed + int analog_in = analogRead(ANALOG_IN); + stepperq.setMaxSpeed(analog_in); + delay(50); +} diff --git a/Lib/stepperQ/Examples/Quickstop/Quickstop.ino b/Lib/stepperQ/Examples/Quickstop/Quickstop.ino new file mode 100644 index 0000000..bd6aaf2 --- /dev/null +++ b/Lib/stepperQ/Examples/Quickstop/Quickstop.ino @@ -0,0 +1,30 @@ +// Quickstop.pde +// -*- mode: C++ -*- +// +// Check stop handling. +// Calls stop() while the stepper is travelling at full speed, causing +// the stepper to stop as quickly as possible, within the constraints of the +// current acceleration. +// +// Copyright (C) 2015 Alexander Chestnov + +#include + + +int dir_pin = 7; +int step_pin = 6 ; + +void setup() +{ + stepperq.setMaxSpeed(150); + stepperq.setAcceleration(900); + stepperq.moveTo(4000); + stepperq.start(); +} + +void loop() +{ + delay(1000); + stepperq.stop(); + +} diff --git a/Lib/stepperQ/Examples/RunToPos/Blocking.ino~ b/Lib/stepperQ/Examples/RunToPos/Blocking.ino~ new file mode 100644 index 0000000..01c2329 --- /dev/null +++ b/Lib/stepperQ/Examples/RunToPos/Blocking.ino~ @@ -0,0 +1,35 @@ + +#include + +int c = 0; + +int dir_pin = 7; +int step_pin = 6 ; + +void setup() { + + stepperq.init(dir_pin,step_pin); + stepperq.setMaxSpeed(200); + stepperq.setAcceleration(9900); + +} +void loop () { + + if ( stepperq.distanceToGo() == 0 ) { + + if (c%2 == 0) + { + stepperq.moveTo(400); + stepperq.start(); + } + else { + stepperq.moveTo(0); + stepperq.start(); + } + + c++; + } + + delay(100); + +} diff --git a/Lib/stepperQ/Examples/RunToPos/RunToPos.ino b/Lib/stepperQ/Examples/RunToPos/RunToPos.ino new file mode 100644 index 0000000..13db89e --- /dev/null +++ b/Lib/stepperQ/Examples/RunToPos/RunToPos.ino @@ -0,0 +1,50 @@ + +// Copyright (C) 2015 Alexander Chestnov + +#include + +int c = 0; + + + +int dir_pin = 7; +int step_pin = 6 ; + +void setup() { + + stepperq.init(dir_pin,step_pin); + stepperq.setMaxSpeed(200); + stepperq.setAcceleration(9900); + +} +void loop () { + + if ( stepperq.distanceToGo() == 0 ) { + + switch (c%4) { + + case 0: + stepperq.moveTo(200); + stepperq.start(); + break; + case 1: + stepperq.moveTo(400); + stepperq.start(); + break; + case 2: + stepperq.moveTo(200); + stepperq.start(); + break; + case 3: + stepperq.moveTo(0); + stepperq.start(); + break; + + } + c++; + } + //do Something other... + + delay(100); + +} diff --git a/Lib/stepperQ/README.md b/Lib/stepperQ/README.md index 7717138..6bf73f0 100644 --- a/Lib/stepperQ/README.md +++ b/Lib/stepperQ/README.md @@ -1,2 +1,53 @@ # stepperQ Arduino SteperMotor Driver. Acceleration and intterupt + + +ESP 8266 Support Added. + + +Testet with: +WemosD1 Mini +Arduino Nano +Arduino Uno. + +Drivers: +-TB5660 Driver ( Not gut very Idea) +-DRV8825 https://www.pololu.com/product/2133 +-L298E https://coeleveld.com/arduino-stepper-l298n/ ( Not reccomendet!!!) +-uln2003a https://coeleveld.com/arduino-stepper-uln2003a/ + + +Example Code: + +#include + +int dir_pin = D3; +int step_pin = D4 ; + +void setup() { + + stepperq.init(dir_pin,step_pin); + stepperq.setMaxSpeed(800); + stepperq.setAcceleration(4800); + stepperq.moveTo(80000); + + stepperq.start(); + delay(40000); +} + +void loop() { + //your code hier. + delay(2000); + +} +For contueres run: + + +void loop () { + + if ( stepperq.distanceToGo() <4000 ) { + stepperq.move(8000); + } + //your code hier. + +} diff --git a/Lib/stepperQ/stepperQ.cpp b/Lib/stepperQ/stepperQ.cpp index ac2bb2e..fd7a6b8 100644 --- a/Lib/stepperQ/stepperQ.cpp +++ b/Lib/stepperQ/stepperQ.cpp @@ -3,15 +3,21 @@ #include "stepperQ.h" - StepperQ stepperq; // preinstatiate +#ifndef ESP8266 ISR(TIMER1_OVF_vect) // interrupt service routine that wraps a user defined function supplied by attachInterrupt { - stepperq.isrCallback(); + stepperq.isrCallback(); } +#else - +void inline timer0ISR(void){ + + stepperq.isrCallback(); + +} +#endif void StepperQ::init(uint8_t dirpin, uint8_t steppin) { @@ -20,48 +26,55 @@ void StepperQ::init(uint8_t dirpin, uint8_t steppin) _maxSpeed = 1.0; _acceleration = 1.0; _sqrt_twoa = 1.0; - - + + _pin[1] = dirpin; _pin[0] = steppin; - - + // NEW _n = 0; _c0 = 0.0; _cn = 0.0; _cmin = 1.0; _direction = DIRECTION_CCW; - + + TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); // brokly + pinMode(_enablePin, OUTPUT); - + pinMode(_pin[1], OUTPUT); pinMode(_pin[0], OUTPUT); - _debug = false; + +#ifdef DEBUG + Serial.print("\n StepperQ:init "); + Serial.print(" dirPin:"); + Serial.print(dirpin); + Serial.print(" StepPin :"); + Serial.print(steppin); + + +#endif + + } void StepperQ::init(uint8_t pin1, uint8_t pin2,uint8_t pin3, uint8_t pin4,uint8_t interface) { - - _interface = interface; - + _pin[2] = pin3; + _pin[3] = pin4; pinMode(_pin[2], OUTPUT); pinMode(_pin[3], OUTPUT); - _pin[2] = pin3; - _pin[3] = pin4; - init (pin1,pin2); + + init (pin2,pin1); //Serial.print("\n init4"); //Serial.print(_interface); - } - - void StepperQ::moveTo(long absolute) { if (_targetPos != absolute) { - _targetPos = absolute; + _targetPos = absolute; } } @@ -89,198 +102,252 @@ long StepperQ::currentPosition() // Sets speed to 0 void StepperQ::setCurrentPosition(long position) { - - _currentPos = position; - + _targetPos = _currentPos = position; + _n = 0; } - - void StepperQ::stop() { - if (_debug) - Serial.print("\n StepperQ:stop"); - move(abs(_n)); - //_direction ==1 ? abs(_n):-abs(_n); - +#ifdef DEBUG + Serial.print("\n StepperQ:stop"); +#endif + move(_direction ==DIRECTION_CW ? abs(_n):-abs(_n)); + } void StepperQ::start(){ - - if (_n==0 && distanceToGo() != 0) { - - _cn = _c0; - _direction = (distanceToGo() > 0) ? DIRECTION_CW : DIRECTION_CCW; - changeDirection() ; - step(HIGH); - setPeriod(_cn); - _n++; - initTimer(_cn); - if (_debug) - Serial.print("\n first step"); - delayMicroseconds(_cmin); - step(LOW); - } - + + if (_n==0 && distanceToGo() != 0) { + _cn = _c0; + _direction = (distanceToGo() > 0) ? DIRECTION_CW : DIRECTION_CCW; + changeDirection() ; + step(HIGH); + setPeriod(_cn); + _n++; + _currentPos += _direction; + initTimer(_cn); +#ifdef DEBUG + Serial.print("\n first step"); +#endif + delayMicroseconds(_cmin); + step(LOW); + } + } void StepperQ::setMaxSpeed(float speed) { - _maxSpeed = speed; - _cmin = 1000000.0 / speed; - _stepsToStop = (long)((speed * speed) / (2.0 * _acceleration)); - + _maxSpeed = speed; + _cmin = 1000000.0 / speed; + _c0 = max(_c0,_cmin); + _stepsToStop = (long)((speed * speed) / (2.0 * _acceleration)); + +#ifdef DEBUG + Serial.print("\n setMaxSpeed _cmin = "); + Serial.print(_cmin); + Serial.print("_c0 = "); + Serial.print(_c0); + +#endif + } float StepperQ::maxSpeed() { - return _maxSpeed; + return _maxSpeed; } float StepperQ::speed() { - return 1000000.0/_cn; + return 1000000.0/_cn; } void StepperQ::setAcceleration(float acceleration) { if (acceleration == 0.0) - return; + return; if (_acceleration != acceleration) { - // Recompute _n per Equation 17 - _n = _n * (_acceleration / acceleration); + // Recompute _n per Equation 17 + _n = _n * (_acceleration / acceleration); _stepsToStop = (long)((_maxSpeed * _maxSpeed) / (2.0 * _acceleration)); - // New c0 per Equation 7 - _c0 = sqrt(2.0 / acceleration) * 1000000.0; - _acceleration = acceleration; + // New c0 per Equation 7 + _c0 = sqrt(2.0 / acceleration) * 1000000.0; + _c0 = max(_c0,_cmin); + + _acceleration = acceleration; } +#ifdef DEBUG + Serial.print("\n setAcceleration _c0 = "); + Serial.print(_c0); + +#endif } float StepperQ::getAcceleration() { - - return _acceleration; - - } + + return _acceleration; + +} long StepperQ::stepsToStop () { - return abs(_n); + return abs(_n); } long StepperQ::maxstepsToStop() { - - return _stepsToStop ; + + return _stepsToStop ; } void StepperQ::isrCallback(){ - - step(HIGH); - _currentPos += _direction; - calculateSpeed(); - + + if (distanceToGo() == 0) { + _n = 0; + stopTimer(); + return; + } + + step(HIGH); + _currentPos += _direction; + calculateSpeed(); + step(LOW); +} -} void StepperQ::calculateSpeed() { - + long distanceTo = distanceToGo(); // +ve is clockwise from curent location - -if (_debug) { - Serial.print("\n n="); + +#ifdef DEBUG + // Serial.print("\n m="); + // Serial.print(millis()); + Serial.print("\n calcSp _currentPos: "); + Serial.print(_currentPos); + Serial.print(" _targetPos:"); + Serial.print(_targetPos); + Serial.print(" _n:"); Serial.print(_n); - Serial.print(" _cn="); + Serial.print(" _c0. "); + Serial.print(_c0); + Serial.print(" _cn:"); Serial.print(_cn); -} - float cnalt= _cn; - if (distanceTo == 0 && _n <= 1) - { - // We are at the target and its time to stop - _n = 0; + + Serial.print(" _cmin:"); + Serial.print(_cmin); +#endif + float _cnew = _cn; + if (distanceTo == 0 && _n <= 1) { + // We are at the target and its time to stop + _n = 0; stopTimer(); - if (_debug) { - Serial.print(" Stopped"); - } - return; +#ifdef DEBUG + Serial.print(" Stopped"); +#endif + return; } - if (distanceTo > 0) - { - // We are anticlockwise from the target - // Need to go clockwise from here, maybe decelerate now - if (_n > 0) - { - // Currently accelerating, need to decel now? Or maybe going the wrong way? - if ((_n >= distanceTo) || _direction == DIRECTION_CCW) - _n = -_n ; // Start deceleration - } - else if (_n < 0) - { - // Currently decelerating, need to accel again? - if ((-_n < distanceTo) && _direction == DIRECTION_CW) - _n = -_n; // Start accceleration - } + + if (distanceTo > 0) { + // We are anticlockwise from the target + // Need to go clockwise from here, maybe decelerate now + if (_n > 0) { + // Currently accelerating, need to decel now? Or maybe going the wrong way? + if ((_n >= distanceTo) || _direction == DIRECTION_CCW ) { +#ifdef DEBUG + Serial.print(" Start D "); +#endif + _n = -_n ; // Start deceleration + } + } + else if (_n < 0) + { + // Currently decelerating, need to accel again? + if ((-_n < distanceTo) && _direction == DIRECTION_CW) { +#ifdef DEBUG + Serial.print(" A again"); +#endif + _n = -_n; // Start accceleration + } + } } else if (distanceTo < 0) { - // We are clockwise from the target - // Need to go anticlockwise from here, maybe decelerate - if (_n > 0) - { - // Currently accelerating, need to decel now? Or maybe going the wrong way? - if ((_n >= -distanceTo) || _direction == DIRECTION_CW) - _n = -_n; // Start deceleration - } - else if (_n < 0) - { - // Currently decelerating, need to accel again? - if ((-_n < -distanceTo) && _direction == DIRECTION_CCW) - _n = -_n; // Start accceleration - } + // We are clockwise from the target + // Need to go anticlockwise from here, maybe decelerate + if (_n > 0) + { + // Currently accelerating, need to decel now? Or maybe going the wrong way? + if ((_n >= -distanceTo) || _direction == DIRECTION_CW) { +#ifdef DEBUG Serial.print(" Start D "); +#endif + _n = -_n; // Start deceleration + } + } + else if (_n < 0) + { + // Currently decelerating, need to accel again? + if ((-_n < -distanceTo) && _direction == DIRECTION_CCW) { +#ifdef DEBUG Serial.print(" Start A "); +#endif + _n = -_n; // Start accceleration + } + } } - - if (_n == 0) + + if (_n == 0) { - //Serial.print("\n first step"); - // First step from stopped - _cn = _c0; - _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; - changeDirection() ; - setPeriod(_cn); - _n++; + //Serial.print("\n first step"); + // First step from stopped + _cn = _c0; + _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; + changeDirection() ; + setPeriod(max(_cn, _cmin)); + _n++; } else if (_n > 0 && _cn > _cmin ) { - - -// Serial.print(" 2222_n="); -// Serial.print(_n); - // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). - _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 - _cn = max(_cn, _cmin); - - _n++; + // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). + _cnew = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 + if (_cnew > _cmin) { + //_cn = max(_cn, _cmin); + _cn= _cnew; + _n++; + } + +#ifdef DEBUG + Serial.print(" a "); +#endif } else if (_n > 0 && _cn < _cmin) { // speed was changed. Need no decel - - _cn = _cn + ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 - _n--; - } - - else if (_n <= 0) { - - _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 - _n++; - } - - // if (abs(cnalt - _cn )>10) { - // setPeriod(_cn); - // } - setPeriod(_cn); - - + + _cn = _cn + ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 + _cnew =_cn; + _n--; +#ifdef DEBUG + Serial.print(" D "); +#endif + } + + else if (_n <= 0) { + + _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 + _cnew = _cn; + +#ifdef DEBUG + Serial.print(" C "); +#endif + _n++; + } + + setPeriod(max(_cnew, _cmin)); +#ifdef DEBUG + Serial.print(" _cn: "); + Serial.print(_cn); +#endif + } void StepperQ::changeDirection() { - if (_reverse) { - digitalWrite(_pin[1], _direction ==1 ? 0:1); - } else { - //Normal - digitalWrite(_pin[1], _direction ==1 ? 1:0); - } - + if (_reverse) { + digitalWrite(_pin[1], _direction ==1 ? 0:1); + } else { + //Normal + digitalWrite(_pin[1], _direction ==1 ? 1:0); + } + } int StepperQ::getDirection() @@ -288,46 +355,87 @@ int StepperQ::getDirection() return _direction; } void StepperQ::setDirOrder(boolean reverse ) { - - _reverse =reverse ; + + _reverse =reverse ; } void StepperQ::setPeriod(long microseconds) { - long cycles = (F_CPU * microseconds) / 2000000; // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2 - if(cycles < RESOLUTION) clockSelectBits = _BV(CS10); // no prescale, full xtal - else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11); // prescale by /8 - else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11) | _BV(CS10); // prescale by /64 - else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12); // prescale by /256 - else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12) | _BV(CS10); // prescale by /1024 - else cycles = RESOLUTION - 1, clockSelectBits = _BV(CS12) | _BV(CS10); // request was out of bounds, set as maximum - ICR1 = cycles; // ICR1 is TOP in p & f correct pwm mode - TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); - TCCR1B |= clockSelectBits; // reset clock select register +#ifndef ESP8266 + + long cycles = (F_CPU * microseconds) / 2000000; // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2 + if(cycles < RESOLUTION) clockSelectBits = _BV(CS10); // no prescale, full xtal + else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11); // prescale by /8 + else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11) | _BV(CS10); // prescale by /64 + else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12); // prescale by /256 + else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12) | _BV(CS10); // prescale by /1024 + else cycles = RESOLUTION - 1, clockSelectBits = _BV(CS12) | _BV(CS10); // request was out of bounds, set as maximum + ICR1 = cycles; // ICR1 is TOP in p & f correct pwm mode + TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); + TCCR1B |= clockSelectBits; // reset clock select register +#else + timer0_write(ESP.getCycleCount() +clockCyclesPerMicrosecond()*microseconds); +#endif } void StepperQ::initTimer(long microseconds) { - TCCR1A = 0; // clear control register A - TCCR1B = _BV(WGM13); // set mode as phase and frequency correct pwm, stop the timer - if(microseconds > 0) setPeriod(microseconds); - TIMSK1 = _BV(TOIE1); // sets the timer overflow interrupt enable bit - sei(); // ensures that interrupts are globally enabled - TCCR1B |= clockSelectBits; +#ifndef ESP8266 + + TCCR1A = 0; // clear control register A + TCCR1B = _BV(WGM13); // set mode as phase and frequency correct pwm, stop the timer + if(microseconds > 0) setPeriod(microseconds); + TIMSK1 = _BV(TOIE1); // sets the timer overflow interrupt enable bit + sei(); // ensures that interrupts are globally enabled + TCCR1B |= clockSelectBits; +#else + timer0_isr_init(); + timer0_attachInterrupt(timer0ISR); + setPeriod (microseconds); +#endif } void StepperQ::stopTimer() -{ - TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); // clears all clock selects bits +{ +#ifndef ESP8266 + + TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); // clears all clock selects bits +#else + timer0_detachInterrupt(); +#endif + + switch (_interface) + { + case DRIVER: + step1(LOW); + break; + case FULL2WIRE: + setOutputPins(0b00); + break; + case FULL3WIRE: + setOutputPins(0b000); + break; + case FULL4WIRE: + setOutputPins(0b0000); + break; + case HALF3WIRE: + setOutputPins(0b000); + break; + + case HALF4WIRE: + setOutputPins(0b0000); + break; + } + } -void StepperQ::debug( boolean debug) { +//void StepperQ::debug( boolean debug) { - _debug= debug; +// _debug= debug; -} +//} void StepperQ::setEnablePin(uint8_t enablePin) { - + _enablePin = enablePin; } @@ -335,46 +443,46 @@ void StepperQ::setEnablePin(uint8_t enablePin) { // Subclasses can override void StepperQ::step(uint8_t first) { - //Serial.print("step:"); - //Serial.print(_interface); - - if (first == HIGH) { - switch (_interface) - { - - case DRIVER: - step1(first); - break; - - case FULL2WIRE: - step2(_currentPos); - break; - - case FULL3WIRE: - step3(_currentPos); - break; - - case FULL4WIRE: - step4(_currentPos); - break; - - case HALF3WIRE: - step6(_currentPos); - break; - - case HALF4WIRE: - step8(_currentPos); - break; - } - } else { - switch (_interface) - { - - case DRIVER: - step1(first); - break; - } //switch - + //Serial.print("step:"); + //Serial.print(_interface); + + if (first == HIGH) { + switch (_interface) + { + + case DRIVER: + step1(first); + break; + + case FULL2WIRE: + step2(_currentPos); + break; + + case FULL3WIRE: + step3(_currentPos); + break; + + case FULL4WIRE: + step4(_currentPos); + break; + + case HALF3WIRE: + step6(_currentPos); + break; + + case HALF4WIRE: + step8(_currentPos); + break; + } + } else { + switch (_interface) + { + + case DRIVER: + step1(first); + break; + } //switch + } // else } @@ -385,11 +493,35 @@ void StepperQ::step(uint8_t first) void StepperQ::setOutputPins(uint8_t mask) { uint8_t numpins = 2; - if (_interface == FULL4WIRE || _interface == HALF4WIRE) - numpins = 4; + if (_interface == FULL4WIRE || _interface == HALF4WIRE) { + numpins = 4; + } + if (_interface == FULL3WIRE || _interface == HALF3WIRE) { + numpins = 3; + } uint8_t i; - for (i = 0; i < numpins; i++) - digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH) : (LOW )); + +#ifdef DEBUG + Serial.print("\n setOutputPins "); + Serial.print("mask "); + + +#endif + + + for (i = 0; i < numpins; i++) { + digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH) : (LOW )); + +#ifdef DEBUG + + //Serial.print(" pin="); + //Serial.print(_pin[i]); + Serial.print(" "); + Serial.print((mask & (1 << i)) ? (HIGH) : (LOW )); +#endif + + } + } @@ -410,21 +542,21 @@ void StepperQ::step2(long step) { switch (step & 0x3) { - case 0: /* 01 */ - setOutputPins(0b10); - break; - - case 1: /* 11 */ - setOutputPins(0b11); - break; - - case 2: /* 10 */ - setOutputPins(0b01); - break; - - case 3: /* 00 */ - setOutputPins(0b00); - break; + case 0: /* 01 */ + setOutputPins(0b10); + break; + + case 1: /* 11 */ + setOutputPins(0b11); + break; + + case 2: /* 10 */ + setOutputPins(0b01); + break; + + case 3: /* 00 */ + setOutputPins(0b00); + break; } } // 3 pin step function @@ -434,18 +566,18 @@ void StepperQ::step3(long step) { switch (step % 3) { - case 0: // 100 - setOutputPins(0b100); - break; - - case 1: // 001 - setOutputPins(0b001); - break; - - case 2: //010 - setOutputPins(0b010); - break; - + case 0: // 100 + setOutputPins(0b100); + break; + + case 1: // 001 + setOutputPins(0b001); + break; + + case 2: //010 + setOutputPins(0b010); + break; + } } @@ -454,27 +586,28 @@ void StepperQ::step3(long step) // Subclasses can override void StepperQ::step4(long step) { - - // Serial.print("step4"); - // Serial.print(step); - +#ifdef DEBUG + Serial.print("\n step4="); + Serial.print(step); +#endif + switch (step & 0x3) { - case 0: // 1010 - setOutputPins(0b0101); - break; - - case 1: // 0110 - setOutputPins(0b0110); - break; - - case 2: //0101 - setOutputPins(0b1010); - break; - - case 3: //1001 - setOutputPins(0b1001); - break; + case 0: // 1010 + setOutputPins(0b0101); + break; + + case 1: // 0110 + setOutputPins(0b0110); + break; + + case 2: //0101 + setOutputPins(0b1010); + break; + + case 3: //1001 + setOutputPins(0b1001); + break; } } @@ -485,30 +618,30 @@ void StepperQ::step6(long step) { switch (step % 6) { - case 0: // 100 - setOutputPins(0b100); - break; - - case 1: // 101 - setOutputPins(0b101); - break; - - case 2: // 001 - setOutputPins(0b001); - break; - - case 3: // 011 - setOutputPins(0b011); - break; - - case 4: // 010 - setOutputPins(0b010); - break; - - case 5: // 011 - setOutputPins(0b110); - break; - + case 0: // 100 + setOutputPins(0b100); + break; + + case 1: // 101 + setOutputPins(0b101); + break; + + case 2: // 001 + setOutputPins(0b001); + break; + + case 3: // 011 + setOutputPins(0b011); + break; + + case 4: // 010 + setOutputPins(0b010); + break; + + case 5: // 011 + setOutputPins(0b110); + break; + } } @@ -519,38 +652,60 @@ void StepperQ::step8(long step) { switch (step & 0x7) { - case 0: // 1000 - setOutputPins(0b0001); - break; - - case 1: // 1010 - setOutputPins(0b0101); - break; - - case 2: // 0010 - setOutputPins(0b0100); - break; - - case 3: // 0110 - setOutputPins(0b0110); - break; - - case 4: // 0100 - setOutputPins(0b0010); - break; - - case 5: //0101 - setOutputPins(0b1010); - break; - - case 6: // 0001 - setOutputPins(0b1000); - break; - - case 7: //1001 - setOutputPins(0b1001); - break; + case 0: // 1000 + setOutputPins(0b0001); + break; + + case 1: // 1010 + setOutputPins(0b0101); + break; + + case 2: // 0010 + setOutputPins(0b0100); + break; + + case 3: // 0110 + setOutputPins(0b0110); + break; + + case 4: // 0100 + setOutputPins(0b0010); + break; + + case 5: //0101 + setOutputPins(0b1010); + break; + + case 6: // 0001 + setOutputPins(0b1000); + break; + + case 7: //1001 + setOutputPins(0b1001); + break; } } +void StepperQ::printStatus() { + + Serial.print("\n StepperQ "); + Serial.print(" _currentPos: "); + Serial.print(_currentPos); + Serial.print(" _targetPos:"); + Serial.print(_targetPos); + Serial.print(" _n:"); + Serial.print(_n); + Serial.print(" _c0. "); + Serial.print(_c0); + Serial.print(" _cn:"); + Serial.print(_cn); + + Serial.print(" _cmin:"); + Serial.print(_cmin); + + Serial.print(" speed:"); + Serial.print(speed()); + + +} diff --git a/Lib/stepperQ/stepperQ.h b/Lib/stepperQ/stepperQ.h index 1d00cc0..5440b77 100644 --- a/Lib/stepperQ/stepperQ.h +++ b/Lib/stepperQ/stepperQ.h @@ -9,32 +9,38 @@ #include #endif +#ifndef ESP8266 #include #include +#endif + #define RESOLUTION 65536 // Timer1 is 16 bit // These defs cause trouble on some versions of Arduino #undef round +////#define DEBUG 1 + class StepperQ { - /// \brief Symbolic names for number of pins. + /// \brief Symbolic names for number of pins. /// Use this in the pins argument the AccelStepper1 constructor to /// provide a symbolic name for the number of pins /// to use. +public: typedef enum { - DRIVER = 1, ///< Stepper Driver, 2 driver pins required - FULL2WIRE = 2, ///< 2 wire stepper, 2 motor pins required - FULL3WIRE = 3, ///< 3 wire stepper, such as HDD spindle, 3 motor pins required + DRIVER = 1, ///< Stepper Driver, 2 driver pins required + FULL2WIRE = 2, ///< 2 wire stepper, 2 motor pins required + FULL3WIRE = 3, ///< 3 wire stepper, such as HDD spindle, 3 motor pins required FULL4WIRE = 4, ///< 4 wire full stepper, 4 motor pins required - HALF3WIRE = 6, ///< 3 wire half stepper, such as HDD spindle, 3 motor pins required - HALF4WIRE = 8 ///< 4 wire half stepper, 4 motor pins required + HALF3WIRE = 6, ///< 3 wire half stepper, such as HDD spindle, 3 motor pins required + HALF4WIRE = 8 ///< 4 wire half stepper, 4 motor pins required } MotorInterfaceType; - -public: - void init(uint8_t dirpin = 2, uint8_t steppin = 3); - void init( uint8_t pin1 , uint8_t pin2 , uint8_t pin3, uint8_t pin4,uint8_t interface = StepperQ::FULL4WIRE ); + + void init(uint8_t dirpin = 2, uint8_t steppin = 3); + void init( uint8_t pin1 , uint8_t pin2 , uint8_t pin3, uint8_t pin4,uint8_t interface = StepperQ::FULL4WIRE ); + /// Set the target position. The run() function will try to move the motor /// from the current position to the target position set by the most /// recent call to this function. Caution: moveTo() also recalculates the speed for the next step. @@ -42,49 +48,50 @@ class StepperQ /// \param[in] absolute The desired absolute position. Negative is /// anticlockwise from the 0 position. void moveTo(long absolute); - + /// Set the target position relative to the current position /// \param[in] relative The desired position relative to the current position. Negative is /// anticlockwise from the current position. void move(long relative); - /// Sets the maximum permitted speed. the run() function will accelerate + + /// Sets the maximum permitted speed. the run() function will accelerate /// up to the speed set by this function. /// \param[in] speed The desired maximum speed in steps per second. Must /// be > 0. Caution: Speeds that exceed the maximum speed supported by the processor may - /// Result in non-linear accelerations and decelerations. + /// result in non-linear accelerations and decelerations. void setMaxSpeed(float speed); - + // returns MaxSpeed float maxSpeed(); //return currentSpeed float speed(); - - -/// Sets the acceleration and deceleration parameter. + + + /// Sets the acceleration and deceleration parameter. /// \param[in] acceleration The desired acceleration in steps per second /// per second. Must be > 0.0. This is an expensive call since it requires a square /// root to be calculated. Dont call more ofthen than needed void setAcceleration(float acceleration); - -/// gets the acceleration and deceleration parameter. + + /// gets the acceleration and deceleration parameter. /// acceleration The desired acceleration in steps per second - float getAcceleration(); - + float getAcceleration(); + /// The distance from the current position to the target position. /// \return the distance from the current position to the target position /// in steps. Positive is clockwise from the current position. long distanceToGo(); - + /// The most recently set target position. /// \return the target position /// in steps. Positive is clockwise from the 0 position. long targetPosition(); - + /// The currently motor position. /// \return the current motor position /// in steps. Positive is clockwise from the 0 position. long currentPosition(); - + /// Resets the current position of the motor, so that wherever the motor /// happens to be right now is considered to be the new 0 position. Useful /// for setting a zero position on a stepper after an initial hardware @@ -93,7 +100,7 @@ class StepperQ /// \param[in] position The position in steps of wherever the motor /// happens to be right now. void setCurrentPosition(long position); - + /// Sets the enable pin number for stepper drivers. /// 0xFF indicates unused (default). /// Otherwise, if a pin is set, the pin will be turned on when @@ -102,33 +109,36 @@ class StepperQ /// \param[in] enablePin Arduino digital pin number for motor enable /// \sa setPinsInverted void setEnablePin(uint8_t enablePin = 0xff); - - void setDirOrder(boolean reverse ); -/// Sets a new target position that causes the stepper + + void setDirOrder(boolean reverse ); + /// Sets a new target position that causes the stepper /// to stop as quickly as possible, using to the current speed and acceleration parameters. void stop(); - + void start(); long stepsToStop(); long maxstepsToStop(); - + void isrCallback(); - void debug( boolean debug); virtual int getDirection(); + + virtual void printStatus(); + protected: - + /// \brief Direction indicator /// Symbolic names for the direction the motor is turning typedef enum { - DIRECTION_CCW = -1, ///< Clockwise + DIRECTION_CCW = -1, ///< Clockwise DIRECTION_CW = 1 ///< Counter-Clockwise } Direction; - /// Low level function to set the motor output pins + /// Low level function to set the motor output pins /// bit 0 of the mask corresponds to _pin[0] /// bit 1 of the mask corresponds to _pin[1] /// You can override this to impment, for example serial chip output insted of using the /// output pins directly +public: virtual void setOutputPins(uint8_t mask); /// Called to execute a step. Only called when a new step is /// required. Subclasses may override to implement new stepping @@ -136,56 +146,57 @@ class StepperQ /// number of pins defined for the stepper. /// \param[in] step The current step phase number (0 to 7) virtual void step(uint8_t first); - virtual void changeDirection(); - - - - /// Called to execute a step on a stepper driver (ie where pins == 1). Only called when a new step is + virtual void changeDirection(); + + + + + /// Called to execute a step on a stepper driver (ie where pins == 1). Only called when a new step is /// required. Subclasses may override to implement new stepping /// interfaces. The default sets or clears the outputs of Step pin1 to step, /// and sets the output of _pin2 to the desired direction. The Step pin (_pin1) is pulsed for 1 microsecond /// which is the minimum STEP pulse width for the 3967 driver. /// \param[in] step The current step phase number (0 to 7) virtual void step1(uint8_t step); - + /// Called to execute a step on a 2 pin motor. Only called when a new step is /// required. Subclasses may override to implement new stepping /// interfaces. The default sets or clears the outputs of pin1 and pin2 /// \param[in] step The current step phase number (0 to 7) virtual void step2(long step); - + /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is /// required. Subclasses may override to implement new stepping /// interfaces. The default sets or clears the outputs of pin1, pin2, /// pin3 /// \param[in] step The current step phase number (0 to 7) virtual void step3(long step); - + /// Called to execute a step on a 4 pin motor. Only called when a new step is /// required. Subclasses may override to implement new stepping /// interfaces. The default sets or clears the outputs of pin1, pin2, /// pin3, pin4. /// \param[in] step The current step phase number (0 to 7) virtual void step4(long step); - + /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is /// required. Subclasses may override to implement new stepping /// interfaces. The default sets or clears the outputs of pin1, pin2, /// pin3 /// \param[in] step The current step phase number (0 to 7) virtual void step6(long step); - + /// Called to execute a step on a 4 pin half-steper motor. Only called when a new step is /// required. Subclasses may override to implement new stepping /// interfaces. The default sets or clears the outputs of pin1, pin2, /// pin3, pin4. /// \param[in] step The current step phase number (0 to 7) virtual void step8(long step); - - + + private: - - + + void setPeriod(long microseconds); void initTimer(long microseconds); void stopTimer(); @@ -193,61 +204,60 @@ class StepperQ /// Number of pins on the stepper motor. Permits 2 or 4. 2 pins is a /// bipolar, and 4 pins is a unipolar. uint8_t _interface= 1; // 1, 2, 4, 8, See MotorInterfaceType - + /// Arduino pin number assignments for the 2 or 4 pins required to interface to the /// stepper motor or driver uint8_t _pin[4]; - - // uint8_t _dirpin; - // uint8_t _steppin; - boolean _reverse; - - /// Enable pin for stepper driver, or 0xFF if unused. + + // uint8_t _dirpin; + // uint8_t _steppin; + boolean _reverse; + + /// Enable pin for stepper driver, or 0xFF if unused. uint8_t _enablePin; - - + + /// The current absolution position in steps. long _currentPos; // Steps - + /// The target position in steps. The StepperQ library will move the /// motor from the _currentPos to the _targetPos, taking into account the /// max speed, acceleration and deceleration long _targetPos; // Steps - - /// The maximum permitted speed in steps per second. Must be > 0. - float _maxSpeed; - + + /// The maximum permitted speed in steps per second. Must be > 0. + float _maxSpeed; + /// The acceleration to use to accelerate or decelerate the motor in steps /// per second per second. Must be > 0 float _acceleration; float _sqrt_twoa; // Precomputed sqrt(2*_acceleration) - - /// The step counter for speed calculations + + /// The step counter for speed calculations long _n; - + /// Initial step size in microseconds - //long _c0; + //long _c0; float _c0; - + /// Last step size in microseconds float _cn ; - - // long _cn; - + + // long _cn; + /// Min step size in microseconds based on maxSpeed float _cmin; // at max speed //long _cmin; // at max speed - + /// Current direction motor is spinning in int _direction; // 1 == CW /// Calculatet Steps to Stop. If the Max speed reaches. long _stepsToStop ; - + //timer Vars unsigned char clockSelectBits; - /// set True für Debug output - boolean _debug; + }; extern StepperQ stepperq; diff --git a/README.md b/README.md index 04c8228..ca32fe5 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,8 @@ # J.A.P. -J.A.P. (Just Another Printer) is a tiny firmware for DIY DLP and LCD 3D-printer. -Designed to work on Arduino Uno + CNC shild hardware and Creation Workshop software. +J.A.P. (Just Another Printer) is a tiny firmware for DIY LCD 3D-printer. +Designed to work on Arduino Uno + CNC shild hardware with "Creation Workshop" +or "nanoDLP" software. *** @@ -10,34 +11,40 @@ Designed to work on Arduino Uno + CNC shild hardware and Creation Workshop softw * Z-axis manual control buttons * Z motor manual on/off button * Cooler control -* Servo control +* LED control *** List of Supported G-Codes - Motion: G1 - Pause: G4 - - Motor on/off: M17, M18 - - Cooler on/off: M7(M245), M9(M246) - - Servo control: M3 Sxxx - - UV LED on/off: M107/M106 + - Positioning: G90, G91, G92 + + - Motor on: M17 + - Motor off: M18, M84 + - Cooler on: M7, M245 + - Cooler off: M9, M246 + - UV LED on: M3, M4, M106 + - UV LED off: M5, M107 + - Current position: M114 + *** - - CNC shild v.3 pinout for DLP: - - ![pinout](https://github.com/3DLab-DLP/jap/blob/master/Img/Arduino-CNC-Shield-V3.jpg) - + CNC shild v.3 pinout for LCD: ![pinout](https://github.com/3DLab-DLP/jap/blob/master/Img/JAP_LCD_pinout.jpg) *** - Setup - * #define STP_PER_MM - number of steps per millimeter for your Z-axis - * #define ACCEL_STP - acceleration in steps per second per second - * #define ZBUTTON_STEP - minimum number of steps when Z-axis control button is pressed + Setup: + + * #define STEPS_PER_MM - resolution in steps/mm for Z-axis + * #define MAX_SPEED - maximum speed in mm/s + * #define ACC_Z - acceleration in mm/s2 + * #define Z_MIN_MM - lower Z-axis bound in mm + * #define Z_MAX_MM - upper Z-axis bound in mm + * #define TIMEOUT - stepper idle hold timeout in minutes P.S. Jumper or normally closed endstop needed on Z+. Z+ is not a home! It's only limiter. diff --git a/jap_lcd.ino b/jap_lcd.ino index 77d966f..78d60d2 100644 --- a/jap_lcd.ino +++ b/jap_lcd.ino @@ -1,199 +1,464 @@ -/* - J.A.P. is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - J.A.P. is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. -(c) Vladimir Razin 2017 info@3dlab.su -*/ - -#include - -#define DEFAULTBAUDRATE 115200 -#define MAX_BUF (64) - -#define ACCEL_STP 32000 -#define STP_PER_MM 1600 -#define ZBUTTON_STEP 16 - -char buffer[MAX_BUF]; -int sofar; - -int flag=1; -int led_flag=1; - -int dir_pin = 7; -int step_pin = 4 ; -int led_pin=10; -int zendstop_plus=11; -int ignition=12; -char zbutton_plus=A0; -char zbutton_minus=A1; -char zmotor=A2; - -unsigned long t0; - -//G-code parser begin - -float parsenumber(char code,float val) { - char *ptr=buffer; - while(ptr && *ptr && ptr 0) { - char c=Serial.read(); - if(sofar 1000) sk=4; - } - while (digitalRead(zbutton_minus)== 0) { - stepperq.setMaxSpeed(STP_PER_MM*sk); - stepperq.move(-ZBUTTON_STEP); - stepperq.start(); - if(millis() - t0 > 1000) sk=4; - } - delay(50); - digitalWrite(8,HIGH); - } - } - } - } - else { - stepperq.setMaxSpeed(STP_PER_MM*3); - stepperq.move(-STP_PER_MM); - stepperq.start(); - delay(1000); - digitalWrite(8,HIGH); - } -} - +/* + J.A.P. is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + J.A.P. is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. +(c) Alexander Shvetsov, 2019, shv-box@mail.com + based on work of Vladimir Razin 2017 info@3dlab.su +*/ + +#include + +/// Movement settings +#define STEPS_PER_MM 1600 +//#define MOVE_BT_STEPS 32 // 0.02 mm (32 / 1600) + +#define MAX_SPEED 5 // mm/s +#define ACC_Z 10 // mm/s2 +#define TIMEOUT 10 // minutes + +/// Bounds +#define Z_MIN_MM 0 +#define Z_MAX_MM 185 + +/// Pins +#define EN_PIN 8 +#define DIR_PIN 7 +#define STEP_PIN 4 + +#define MIN_ENDSTOP 11 + +#define LED_PIN 10 +#define COOLER_PIN A3 + +#define BT_LED_PIN 12 // LED toggle button +#define BT_MOTOR_PIN A2 // Motor on/off button +#define BT_UP_PIN A0 // Move up button +#define BT_DOWN_PIN A1 // Move down button + +/// Serial +#define BAUDRATE 115200 +#define BUF_MAX_IDX 255 // Input buffer size - 1 + +/// Button debounce +#define DEBOUNCE 200UL + +/// Debug +//#define DEBUG + +const float TRAVEL_SPEED_MMS = MAX_SPEED; +const float TRAVEL_SPEED_STP = MAX_SPEED * STEPS_PER_MM; +const unsigned long STEPPER_TIMEOUT = 60UL * 1000UL * TIMEOUT; +const float ACC_STEPS_S2 = float(ACC_Z) * STEPS_PER_MM; + +const char MSG_OK[] = "ok"; +const char MSG_ERR[] = "!!"; + +typedef enum { + FAIL = 0, + OK = 1, + UNSUPPORTED = 2, + UNOPERATIONAL = 3 +} CmdStatus; + +/// Stepper +unsigned long motorTime; // Stepper timeout start time + +/// Motor control buttons +bool motorIsOn; +unsigned long motorBtTime; +bool motorBtWasPressed; + +unsigned long moveBtTime; +bool moveBtWasPressed; + +/// LED control buttons +bool ledIsOn; +unsigned long ledBtTime; +bool ledBtWasPressed; + +/// Command input buffer +char buffer[BUF_MAX_IDX + 1]; +int bufBound; // Buffer current bound index + +/// Z positioning +bool isAbsolute; // Move coordinates is absolute or relative. Default is 'true' +float z; // Current Z-axis position +bool isOperational; // Current Z-axis position is actual + +/// Serial input parser +float parseValue(char code, float def) { + char *ptr = buffer; + while(ptr && *ptr && ptr < buffer + bufBound) { + if (*ptr == code) { + return atof(ptr + 1); + } + ptr = strchr(ptr, ' ') + 1; + } + return def; +} + +void enableMotor() { + if (!motorIsOn) { + digitalWrite(EN_PIN, LOW); + motorIsOn = true; + } + motorTime = millis(); +} + +void disableMotor() { + if (motorIsOn) { + digitalWrite(EN_PIN, HIGH); + if (stepperq.distanceToGo() != 0) { + long pos = stepperq.currentPosition(); + stepperq.setCurrentPosition(pos); + z = float(pos) / STEPS_PER_MM; + } + motorIsOn = false; + isOperational = false; + } +} + +CmdStatus processCommand() { +#if defined(DEBUG) + Serial.println(buffer); +#endif + int cmd = parseValue('G', -1); + + switch(cmd) { + case 1: { // G1 Zxxx Fxxx (Z-axis movement) + if (!isOperational) { + return UNOPERATIONAL; + } + + float gZ = parseValue('Z', -1); + if (gZ != -1) { + float newZ = isAbsolute ? gZ : (z + gZ); + /// Check limits + if (newZ < Z_MIN_MM) { + newZ = Z_MIN_MM; + } else if (newZ > Z_MAX_MM) { + newZ = Z_MAX_MM; + } + + float speed = parseValue('F', 0.0) / 60; + if (speed == 0.0 || speed > TRAVEL_SPEED_MMS) { + speed = TRAVEL_SPEED_MMS; + } + + /// Move + enableMotor(); + stepperq.setMaxSpeed(speed * STEPS_PER_MM); + stepperq.move((newZ - z) * STEPS_PER_MM); + stepperq.start(); + + z = newZ; + Serial.println("Z_move_comp"); + } + } + return OK; + + case 4: // G04 Pxxx (Pause P milliseconds) + delay(parseValue('P', 0)); + return OK; + + case 90: // G90 (Set to absolute positioning) + isAbsolute = true; + return OK; + + case 91: // G91 (Set to relative positioning) + isAbsolute = false; + return OK; + + case 92: { // G92 Znnn (Set Z axis position) + z = parseValue('Z', 0); +#if defined(DEBUG) + Serial.println(long(z * STEPS_PER_MM)); +#endif + stepperq.setCurrentPosition(long(z * STEPS_PER_MM)); + enableMotor(); + isOperational = true; + } + return OK; + + default: + break; + } + + cmd = parseValue('M', -1); + switch(cmd) { + case 7: // M7 (Mist coolant on) + case 245: // M245 (Start cooler) + digitalWrite(COOLER_PIN, HIGH); + break; + + case 9: // M9 (Coolant off) + case 246: // M246 (Stop cooler) + digitalWrite(COOLER_PIN, LOW); + break; + + case 17: // M17 (Enable/Power all stepper motors) + enableMotor(); + break; + + case 18: // M18 (Disable all stepper motors) + case 84: // M84 (Stop idle hold) + disableMotor(); + break; + + case 3: // M3 (Spindle On, Clockwise) + case 4: // M4 (Spindle On, Counter-Clockwise) + case 106: { // M106 (Fan On) + ledIsOn = true; + digitalWrite(LED_PIN, LOW); + } + break; + + case 5: // M5 (Spindle Off) + case 107: { // M106 (Fan Off) + ledIsOn = false; + digitalWrite(LED_PIN, HIGH); + } + break; + + case 114: { // M114 (Get current position) + Serial.print(isOperational ? MSG_OK : MSG_ERR); + Serial.print(" C: Z:"); + Serial.print(z); + Serial.print(" Count Z:"); + Serial.println(float(stepperq.currentPosition()) / STEPS_PER_MM); + } + break; + + default: + return UNSUPPORTED; + } + + return OK; +} + +void setup() { + Serial.begin(BAUDRATE); + Serial.println("JAP LCD"); + bufBound = 0; + + stepperq.init(DIR_PIN, STEP_PIN); + stepperq.setAcceleration(ACC_STEPS_S2); + + motorTime = 0; + isAbsolute = true; + z = 0.0; + isOperational = false; + + pinMode(EN_PIN, OUTPUT); + digitalWrite(EN_PIN, HIGH); + motorIsOn = false; + + pinMode(LED_PIN, OUTPUT); + digitalWrite(LED_PIN,HIGH); + ledIsOn = false; + + pinMode(COOLER_PIN, OUTPUT); + digitalWrite(COOLER_PIN,LOW); + + pinMode(MIN_ENDSTOP, INPUT_PULLUP); + + pinMode(BT_UP_PIN, INPUT_PULLUP); + pinMode(BT_DOWN_PIN, INPUT_PULLUP); + moveBtTime = 0; + moveBtWasPressed = false; + + pinMode(BT_MOTOR_PIN, INPUT_PULLUP); + motorBtTime = 0; + motorBtWasPressed = false; + + pinMode(BT_LED_PIN, INPUT_PULLUP); + ledBtTime = 0; + ledBtWasPressed = false; +} + +void loop() { + /// Motor timeout + if (motorIsOn && (millis() - motorTime) > STEPPER_TIMEOUT) { + disableMotor(); + } + + /// Motor button + bool motorBtPressed = digitalRead(BT_MOTOR_PIN) == HIGH; + if (motorBtPressed && !motorBtWasPressed && (millis() - motorBtTime) > DEBOUNCE) { + if (motorIsOn) { + disableMotor(); + } else { + enableMotor(); + } + } + motorBtWasPressed = motorBtPressed; + + /// LED button + bool ledBtPressed = digitalRead(BT_LED_PIN) == HIGH; + if (ledBtPressed && !ledBtWasPressed && (millis() - ledBtTime) > DEBOUNCE) { + ledIsOn = !ledIsOn; + ledBtTime = millis(); + digitalWrite(LED_PIN, ledIsOn ? LOW : HIGH); + } + ledBtWasPressed = ledBtPressed; + + if (digitalRead(MIN_ENDSTOP) == LOW) { + if (stepperq.distanceToGo() != 0) return; + + int count = Serial.available(); + int i = 0; + while (i < count && bufBound < BUF_MAX_IDX) { + char c = Serial.read(); + if (c == '\n') { + buffer[bufBound++] = 0; + String msg = ""; + switch (processCommand()) { + case OK: + msg = MSG_OK; + break; + case UNSUPPORTED: + msg = String(MSG_ERR) + ": Unsupported command \"" + String(buffer) + "\""; + break; + case UNOPERATIONAL: + msg = String(MSG_ERR) + ": Printer is not operational. Z-axis position must be setted. Use G92."; + break; + default: + msg = MSG_ERR; + break; + } + Serial.println(msg); + bufBound = 0; + + } else { + buffer[bufBound++] = c; + } + ++i; + } + + /// Manual moves + if (count == 0) { + bool upBtPressed = digitalRead(BT_UP_PIN) == LOW; + bool downBtPressed = digitalRead(BT_DOWN_PIN) == LOW; + bool moveBtPressed = upBtPressed || downBtPressed; + + if (moveBtPressed && !moveBtWasPressed) { + moveBtTime = millis(); + } + + if (moveBtPressed && (millis() - moveBtTime) > DEBOUNCE) { + bool motorWasOn = motorIsOn; + int moveDirection = upBtPressed ? 1 : -1; + long maxSteps = (moveDirection > 0 ? (Z_MAX_MM - z) : (z - Z_MIN_MM)) * ((long) STEPS_PER_MM); +#if defined(DEBUG) + Serial.print("maxSteps = "); + Serial.println(maxSteps); +#endif + + if (maxSteps > 0L) { + enableMotor(); + + if (moveDirection > 0) { + // Stage 1 + stepperq.setMaxSpeed(0.5 * STEPS_PER_MM); + stepperq.move(moveDirection * STEPS_PER_MM); + stepperq.start(); + while (moveBtPressed && digitalRead(MIN_ENDSTOP) == LOW && stepperq.distanceToGo() != 0) { + delay(50); + moveBtPressed = digitalRead(moveDirection == 1 ? BT_UP_PIN : BT_DOWN_PIN) == LOW; + } + maxSteps -= STEPS_PER_MM; + + // Stage 2 + if (moveBtPressed && digitalRead(MIN_ENDSTOP) == LOW) { + stepperq.setMaxSpeed(STEPS_PER_MM); + stepperq.move(moveDirection * STEPS_PER_MM); + stepperq.start(); + while (moveBtPressed && digitalRead(MIN_ENDSTOP) == LOW && stepperq.distanceToGo() != 0) { + delay(50); + moveBtPressed = digitalRead(moveDirection == 1 ? BT_UP_PIN : BT_DOWN_PIN) == LOW; + } + maxSteps -= STEPS_PER_MM; + } + } + + // Stage 3 + if (moveBtPressed && digitalRead(MIN_ENDSTOP) == LOW) { + stepperq.setMaxSpeed(TRAVEL_SPEED_STP); + stepperq.move(moveDirection * maxSteps); + stepperq.start(); + while (moveBtPressed && digitalRead(MIN_ENDSTOP) == LOW && stepperq.distanceToGo() != 0) { + delay(50); + moveBtPressed = digitalRead(moveDirection == 1 ? BT_UP_PIN : BT_DOWN_PIN) == LOW; + } + } + + // Stop if moving + if (stepperq.distanceToGo() != 0) { + stepperq.stop(); + } + // Let's finish move + while (stepperq.distanceToGo() != 0) delay(5); + + z = float(stepperq.currentPosition()) / STEPS_PER_MM; + + if (!motorWasOn) { + disableMotor(); + } + } + +#if defined(DEBUG) + Serial.print("z = "); + Serial.println(z); +#endif + } + moveBtWasPressed = moveBtPressed; + +// if (moveBtPressed && (millis() - moveBtTime) > DEBOUNCE) { +// isOperational = false; +// bool motorWasOn = motorIsOn; +// int moveDirection = upBtPressed ? 1 : -1; +// const float RATE1 = 0.5; +// const float RATE2 = 1.0; +// const float RATE_MAX = 5.0; +// float rate = RATE1; + +// enableMotor(); + +// while (moveBtPressed && digitalRead(MIN_ENDSTOP) == LOW) { +// if (rate < RATE_MAX) { +// unsigned long btDownTime = millis() - moveBtTime; +// rate = btDownTime < 1000 ? RATE1 : +// (btDownTime < 3000 ? RATE2 : RATE_MAX); +// } + +// stepperq.setMaxSpeed(rate * STEPS_PER_MM); +// stepperq.move(moveDirection * MOVE_BT_STEPS); +// stepperq.start(); + +// moveBtPressed = digitalRead(moveDirection == 1 ? BT_UP_PIN : BT_DOWN_PIN) == LOW; +// } + +// if (!motorWasOn) { +// while (stepperq.distanceToGo() != 0) delay(5); // Let's finish move +// disableMotor(); +// } +// } +// moveBtWasPressed = moveBtPressed; + } + + } else { + enableMotor(); + + stepperq.setMaxSpeed(STEPS_PER_MM * 3); + stepperq.move(-STEPS_PER_MM); + stepperq.start(); + + delay(1000); + disableMotor(); + } +}