Skip to content

M
MarlinKimbra
Commit b3cca544 authored by MagoKimbra's avatar MagoKimbra
Browse files
Options

Add Cartesian Correction 

Add Hysteresis correction
Add Zwobble correction
parent c1e40d24
Show whitespace changes
Inline Side-by-side
Showing with 2415 additions and 1740 deletions
Documentation/GCodes.md
View file @ b3cca544
......@@ -52,6 +52,11 @@
* M84 - Disable steppers until next move, or use S[seconds] to specify an inactivity timeout, after which the steppers will be disabled. S0 to disable the timeout.
* M85 - Set inactivity shutdown timer with parameter S[seconds]. To disable set zero (default)
* M92 - Set axis_steps_per_unit - same syntax as G92
* M96 - Print ZWobble value
* M97 - Set ZWobble parameter M97 A<Amplitude_in_mm> W<period_in_mm> P<phase_in_degrees>
* M98 - Print Hysteresis value
* M99 - Set Hysteresis parameter M99 X<in mm> Y<in mm> Z<in mm> E<in mm>
* M100 - Watch Free Memory (For Debugging Only)
* M104 - Set extruder target temp
* M105 - Read current temp
* M106 - Fan on
......
Documentation/changelog.md
View file @ b3cca544
### Version 4.2.7
* Add M906 Set motor Currents for ALLIGATOR board
* Add M408 JSON OUTPUT
* Add Cartesian Correction Hysteresis and Zwooble
* Bug fix
### Version 4.2.6
* Bug Fix
......
MK/Configuration_Cartesian.h
View file @ b3cca544
......@@ -24,6 +24,7 @@
* - Axis accelleration
* - Homing feedrate
* - Hotend offset
* - Cartesian Correction
*
* Basic-settings can be found in Configuration_Basic.h
* Feature-settings can be found in Configuration_Feature.h
......@@ -423,4 +424,37 @@
//#define HOTEND_OFFSET_Z {0.0, 0.0, 0.0, 0.0} // (in mm) for each hotend, offset of the hotend on the Z axis
/*****************************************************************************************/
/*****************************************************************************************
******************************** CARTESIAN CORRECTION ***********************************
*****************************************************************************************
* *
* New functions, Hysteresis and Zwobble. *
* *
* Hysteresis: *
* These are the extra distances that are performed when an axis changes direction *
* to compensate for any mechanical hysteresis your printer has. *
* Set the parameters width M99 X<in mm> Y<in mm> Z<in mm> E<in mm> *
* *
* ZWobble: *
* How to use it: *
* Set the parameters with M97 A<Amplitude_in_mm> W<period_in_mm> P<phase_in_degrees> *
* KNOWN LIMITATION (by design): if you redefine the Z value during your print *
* (with a G92 for example), the correction *will* screw up *
* How does it work? *
* This class compensates for a wobble of the Z axis that makes the translation *
* rod movement->bed (extruder) movement nonlinear. *
* Instead of assuming Zactual = Zrod, the function assumes that *
* Zaxtual = Zrod + A*sin(w*Zrod + phase). Since the user wants to specify Zactual, *
* we need to invert the formula to obtain Zrod, which is the value that will serve *
* as the input of the motor. *
* *
*****************************************************************************************/
//define HYSTERESIS
//define ZWOBBLE
#define DEFAULT_HYSTERESIS_MM 0, 0, 0, 0 // X, Y, Z, E hysteresis in mm.
#define DEFAULT_ZWOBBLE 0, 0, 0 // A, W, P
/*****************************************************************************************/
#endif
\ No newline at end of file
MK/MK.ino
View file @ b3cca544
......@@ -90,6 +90,10 @@
* or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled. S0 to disable the timeout.
* M85 - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
* M92 - Set axis_steps_per_unit - same syntax as G92
* M96 - Print ZWobble value
* M97 - Set ZWobble parameter M97 A<Amplitude_in_mm> W<period_in_mm> P<phase_in_degrees>
* M98 - Print Hysteresis value
* M99 - Set Hysteresis parameter M99 X<in mm> Y<in mm> Z<in mm> E<in mm>
* M100 - Watch Free Memory (For Debugging Only)
* M104 - Set extruder target temp
* M105 - Read current temp
......
MK/base.h
View file @ b3cca544
......@@ -46,39 +46,23 @@
#include "module/motion/stepper.h"
#include "module/motion/stepper_indirection.h"
#include "module/motion/planner.h"
#include "module/motion/vector_3.h"
#include "module/motion/qr_solve.h"
#include "module/motion/cartesian_correction.h"
#include "module/temperature/temperature.h"
#include "module/temperature/thermistortables.h"
#include "module/lcd/ultralcd.h"
#include "module/lcd/buzzer.h"
#include "module/nextion/Nextion_lcd.h"
#include "module/sd/cardreader.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
#include "module/motion/vector_3.h"
#if ENABLED(AUTO_BED_LEVELING_GRID)
#include "module/motion/qr_solve.h"
#endif
#endif
#include "module/servo/servo.h"
#include "module/watchdog/watchdog.h"
#include "module/blinkm/blinkm.h"
#if MB(ALLIGATOR)
#include "module/alligator/external_dac.h"
#endif
#if ENABLED(USE_WATCHDOG)
#include "module/watchdog/watchdog.h"
#endif
#if HAS(BUZZER)
#include "module/lcd/buzzer.h"
#endif
#if ENABLED(BLINKM)
#include "module/blinkm/blinkm.h"
#endif
#if HAS(SERVOS)
#include "module/servo/servo.h"
#endif
#if HAS(DIGIPOTSS)
#include <SPI.h>
#endif
......@@ -87,4 +71,8 @@
#include "module/fwtest/firmware_test.h"
#endif
#if ENABLED(RFID_MODULE)
#include "module/mfrc522/MFRC522_serial.h"
#endif
#endif
MK/module/HAL/fastio.h
View file @ b3cca544
......@@ -6,6 +6,7 @@
#ifndef _FASTIO_ARDUINO_H
#define _FASTIO_ARDUINO_H
#include <avr/io.h>
/*
utility functions
......
MK/module/MK_Main.cpp
View file @ b3cca544
......@@ -4843,6 +4843,51 @@ inline void gcode_M92() {
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
#if ENABLED(ZWOBBLE)
/**
* M96: Print ZWobble value
*/
inline void gcode_M96() {
zwobble.ReportToSerial();
}
/**
* M97: Set ZWobble value
*/
inline void gcode_M97() {
float zVal = -1, hVal = -1, lVal = -1;
if (code_seen('A')) zwobble.setAmplitude(code_value());
if (code_seen('W')) zwobble.setPeriod(code_value());
if (code_seen('P')) zwobble.setPhase(code_value());
if (code_seen('Z')) zVal = code_value();
if (code_seen('H')) hVal = code_value();
if (code_seen('L')) lVal = code_value();
if (zVal >= 0 && hVal >= 0) zwobble.setSample(zVal, hVal);
if (zVal >= 0 && lVal >= 0) zwobble.setScaledSample(zVal, lVal);
if (lVal > 0 && hVal > 0) zwobble.setScalingFactor(hVal/lVal);
}
#endif // ZWOBBLE
#if ENABLED(HYSTERESIS)
/**
* M98: Print Hysteresis value
*/
inline void gcode_M98() {
hysteresis.ReportToSerial();
}
/**
* M99: Set Hysteresis value
*/
inline void gcode_M99() {
for(uint8_t i = 0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i]))
hysteresis.SetAxis(i, code_value());
}
}
#endif // HYSTERESIS
/**
* M100 Free Memory Watcher
*
......@@ -6470,7 +6515,7 @@ inline void gcode_M428() {
sync_plan_position();
#endif
ECHO_LM(DB, "Offset applied.");
LCD_ALERTMESSAGEPGM("Offset applied.");
LCD_MESSAGEPGM("Offset applied.");
#if HAS(BUZZER)
enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
#endif
......@@ -7455,6 +7500,26 @@ void process_next_command() {
gcode_M85(); break;
case 92: // M92 Set the steps-per-unit for one or more axes
gcode_M92(); break;
#if ENABLED(ZWOBBLE)
case 96: // M96 Print ZWobble value
gcode_M96(); break;
case 97: // M97 Set ZWobble parameter
gcode_M97(); break;
#endif
#if ENABLED(HYSTERESIS)
case 98: // M98 Print Hysteresis value
gcode_M98(); break;
case 99: // M99 Set Hysteresis parameter
gcode_M99(); break;
#endif
#if ENABLED(M100_FREE_MEMORY_WATCHER)
case 100:
gcode_M100(); break;
#endif
case 104: // M104
gcode_M104(); break;
case 105: // M105 Read current temperature
......@@ -7469,12 +7534,6 @@ void process_next_command() {
case 109: // M109 Wait for temperature
gcode_M109(); break;
#if ENABLED(M100_FREE_MEMORY_WATCHER)
case 100:
gcode_M100(); break;
#endif
case 110: break; // M110: Set line number - don't show "unknown command"
case 111: // M111 Set debug level
gcode_M111(); break;
......
MK/module/blinkm/blinkm.cpp
View file @ b3cca544
/*
blinkm.cpp - Library for controlling a BlinkM over i2c
Created by Tim Koster, August 21 2013.
*/
/**
* blinkm.cpp - Library for controlling a BlinkM over i2c
* Created by Tim Koster, August 21 2013.
*/
#include "../../base.h"
#if ENABLED(BLINKM)
#include "blinkm.h"
#include <Wire.h>
#include "blinkm.h"
#include <Wire.h>
void SendColors(byte red, byte grn, byte blu) {
void SendColors(byte red, byte grn, byte blu) {
Wire.begin();
Wire.beginTransmission(0x09);
Wire.write('o'); //to disable ongoing script, only needs to be used once
......@@ -18,7 +18,6 @@ void SendColors(byte red, byte grn, byte blu) {
Wire.write(grn);
Wire.write(blu);
Wire.endTransmission();
}
}
#endif // BLINKM
MK/module/blinkm/blinkm.h
View file @ b3cca544
......@@ -4,8 +4,10 @@
*/
#ifndef BLINKM_H
#define BLINKM_H
#define BLINKM_H
void SendColors(byte red, byte grn, byte blu);
#if ENABLED(BLINKM)
void SendColors(byte red, byte grn, byte blu);
#endif
#endif
MK/module/communication/communication.h
View file @ b3cca544
......@@ -6,6 +6,7 @@
#define ER "Error: " // error for host
#define WT "Wait" // wait for host
#define DB "Echo: " // message for user
#define DEB "Debug: " // message for debug
#define CFG "Config: " // config for host
#define INFO "Info: " // info for host
#define RESEND "Resend: " // resend for host
......
MK/module/lcd/ultralcd_st7920_u8glib_rrd.h
View file @ b3cca544
......@@ -35,8 +35,8 @@ static void ST7920_SWSPI_SND_8BIT(uint8_t val) {
}
}
#define ST7920_CS() {WRITE(ST7920_CS_PIN,1);u8g_10MicroDelay();}
#define ST7920_NCS() {WRITE(ST7920_CS_PIN,0);}
#define ST7920_CS() {WRITE(ST7920_CS_PIN, 1); u8g_10MicroDelay();}
#define ST7920_NCS() {WRITE(ST7920_CS_PIN, 0);}
#define ST7920_SET_CMD() {ST7920_SWSPI_SND_8BIT(0xf8);u8g_10MicroDelay();}
#define ST7920_SET_DAT() {ST7920_SWSPI_SND_8BIT(0xfa);u8g_10MicroDelay();}
#define ST7920_WRITE_BYTE(a) {ST7920_SWSPI_SND_8BIT((uint8_t)((a)&0xf0u));ST7920_SWSPI_SND_8BIT((uint8_t)((a)<<4u));u8g_10MicroDelay();}
......
MK/module/motion/cartesian_correction.cpp 0 → 100644
View file @ b3cca544
/**
* cartesian_correction.cpp
* A class that manages hysteresis by inserting extra plan_buffer_line when necessary
* A class that manages ZWobble
*
* Copyright (c) 2016 MagoKimbra
* Copyright (c) 2013 Francesco Santini
* Copyright (c) 2012 Neil James Martin
*
* Grbl 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.
*
* Grbl 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.
*
* You should have received a copy of the GNU General Public License
* along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include "../../base.h"
#include "cartesian_correction.h"
#ifdef HYSTERESIS
//===========================================================================
Hysteresis hysteresis(DEFAULT_HYSTERESIS_MM);
float axis_shift[NUM_AXIS] = { 0.0f, 0.0f, 0.0f, 0.0f };
//===========================================================================
Hysteresis::Hysteresis(float x_mm, float y_mm, float z_mm, float e_mm) {
m_prev_direction_bits = 0;
Set(x_mm, y_mm, z_mm, e_mm);
}
//===========================================================================
void Hysteresis::Set(float x_mm, float y_mm, float z_mm, float e_mm) {
m_hysteresis_mm[X_AXIS] = x_mm;
m_hysteresis_mm[Y_AXIS] = y_mm;
m_hysteresis_mm[Z_AXIS] = z_mm;
m_hysteresis_mm[E_AXIS] = e_mm;
m_hysteresis_bits = ((m_hysteresis_mm[X_AXIS] != 0.0f) ? (1 << X_AXIS) : 0)
| ((m_hysteresis_mm[Y_AXIS] != 0.0f) ? (1 << Y_AXIS) : 0)
| ((m_hysteresis_mm[Z_AXIS] != 0.0f) ? (1 << Z_AXIS) : 0)
| ((m_hysteresis_mm[E_AXIS] != 0.0f) ? (1 << E_AXIS) : 0);
calcSteps();
}
//===========================================================================
void Hysteresis::SetAxis(uint8_t axis, float mm) {
m_hysteresis_mm[axis] = mm;
if(mm != 0.0f) m_hysteresis_bits |= ( 1 << axis);
else m_hysteresis_bits &= ~( 1 << axis);
calcSteps();
ReportToSerial();
}
//===========================================================================
void Hysteresis::calcSteps() {
for (uint8_t i = 0; i < NUM_AXIS; i++)
m_hysteresis_steps[i] = (long)(m_hysteresis_mm[i] * axis_steps_per_unit[i]);
}
//===========================================================================
void Hysteresis::ReportToSerial() {
ECHO_SMV(DB, "Hysteresis X", m_hysteresis_mm[X_AXIS]);
ECHO_MV(" Y", m_hysteresis_mm[Y_AXIS]);
ECHO_MV(" Z", m_hysteresis_mm[Z_AXIS]);
ECHO_MV(" E", m_hysteresis_mm[E_AXIS]);
ECHO_MV(" SHIFTS: x=", axis_shift[X_AXIS]);
ECHO_MV(" y=", axis_shift[Y_AXIS]);
ECHO_MV(" z=", axis_shift[Z_AXIS]);
ECHO_EMV(" e=", axis_shift[E_AXIS]);
}
//===========================================================================
// direction 0: positive, 1: negative
uint8_t calc_direction_bits(const long* current_position, const long* destination) {
unsigned char direction_bits = 0;
if (destination[X_AXIS] < current_position[X_AXIS])
direction_bits |= (1 << X_AXIS);
if (destination[Y_AXIS] < current_position[Y_AXIS])
direction_bits |= (1 << Y_AXIS);
if (destination[Z_AXIS] < current_position[Z_AXIS])
direction_bits |= (1 << Z_AXIS);
if (destination[E_AXIS] < current_position[E_AXIS])
direction_bits |= (1 << E_AXIS);
return direction_bits;
}
//===========================================================================
uint8_t calc_move_bits(const long* current_position, const long* destination) {
uint8_t move_bits = 0;
if (destination[X_AXIS] != current_position[X_AXIS])
move_bits |= (1 << X_AXIS);
if (destination[Y_AXIS] != current_position[Y_AXIS])
move_bits |= (1 << Y_AXIS);
if (destination[Z_AXIS] != current_position[Z_AXIS])
move_bits |= (1 << Z_AXIS);
if (destination[E_AXIS] != current_position[E_AXIS])
move_bits |= (1 << E_AXIS);
return move_bits;
}
//===========================================================================
// insert a plan_buffer_line if required to handle any hysteresis
void Hysteresis::InsertCorrection(const float x, const float y, const float z, const float e) {
long destination[NUM_AXIS] = {x * axis_steps_per_unit[X_AXIS], y * axis_steps_per_unit[Y_AXIS], z * axis_steps_per_unit[Z_AXIS], e * axis_steps_per_unit[E_AXIS + active_extruder]};
uint8_t direction_bits = calc_direction_bits(position, destination);
uint8_t move_bits = calc_move_bits(position, destination);
// if the direction has changed in any of the axis that need hysteresis corrections...
uint8_t direction_change_bits = (direction_bits ^ m_prev_direction_bits) & move_bits;
if( (direction_change_bits & m_hysteresis_bits) != 0 ) {
// calculate the position to move to that will fix the hysteresis
for(uint8_t axis = 0; axis < NUM_AXIS; axis++) {
// if this axis changed direction...
if(direction_change_bits & (1 << axis)) {
long fix = (((direction_bits & (1 << axis)) != 0) ? -m_hysteresis_steps[axis] : m_hysteresis_steps[axis]);
//... add the hysteresis: move the current position in the opposite direction so that the next travel move is longer
position[axis] -= fix;
axis_shift[axis] += fix;
}
}
}
m_prev_direction_bits = (direction_bits & move_bits) | (m_prev_direction_bits & ~move_bits);
}
#endif // HYSTERESIS
#ifdef ZWOBBLE
// minimum distance within which two distances in mm are considered equal
#define TOLERANCE_MM 0.01
#define DISTANCE(_A,_B) abs((_A) - (_B))
#define EQUAL_WITHIN_TOLERANCE(_A, _B) (DISTANCE(_A, _B) < TOLERANCE_MM)
#define TWOPI 6.28318530718
#define ZACTUAL_IS_SCALED(_I) (zLut[_I][1] < 0)
#define ZACTUAL(_I) (zLut[_I][1] < 0 ? -zLut[_I][1] * m_scalingFactor : zLut[_I][1])
#define ZROD(_I) zLut[_I][0]
#define SET_ZACTUAL(_I,_V) zLut[_I][1] = _V
#define SET_ZROD(_I,_V) zLut[_I][0] = _V
//===========================================================================
ZWobble zwobble(DEFAULT_ZWOBBLE);
//===========================================================================
ZWobble::ZWobble(float _amplitude, float _period, float _phase) :
m_consistent(false),
lastZ(-1.0),
lastZRod(-1.0),
m_scalingFactor(1.0),
m_sinusoidal(true) { Set(_amplitude, _period, _phase); }
//===========================================================================
void ZWobble::Set(float _amplitude, float _period, float _phase) {
setAmplitude(_amplitude);
setPeriod(_period);
setPhase(_phase);
}
//===========================================================================
bool ZWobble::areParametersConsistent() {
if (!m_sinusoidal) {
m_consistent = true; // parameters are always consistent if lut is not sinusoidal
return true; // if the model is not sinusoidal, then don't check for consistency
}
// m_amplitude*m_puls must be less than 1 in order for the function to be invertible (otherwise it would mean that the wobble is so much that the axis goes back)
if (m_puls <= 0 || m_amplitude <= 0 || (m_amplitude * m_puls) >= 1) {
m_consistent = false;
return false;
}
m_consistent = true;
return true;
}
//===========================================================================
void ZWobble::setScaledSample(float zRod, float zScaledLength) {
// We want to be able to correct scaling factor or set it before/after samples, so (ICK) store scaled samples as negative numbers
setSample(zRod, -zScaledLength);
// Make sure we have a non-zero scaling factor
if (!m_scalingFactor) {
m_scalingFactor = 1.0;
}
// Find two scaled samples close to period
float period = TWOPI / m_puls;
int s1 = -1, s2 = -1;
for (int i = 0; i < lutSize; i++) {
if (ZACTUAL_IS_SCALED(i)) {
s1 = s2;
s2 = i;
if (ZROD(s2) >= period) break;
}
}
// Calculate scaling factor so zact[period] = zrod[period]
if (s2 >= 0 && ZACTUAL(s2)) {
// Case 1 - Only one sample
if (s1 < 0) {
m_scalingFactor *= ZROD(s2) / ZACTUAL(s2);
}
// Case 2 - Samples bracketing period (s1 - p - s2): average zact
else if (ZROD(s2) > period) {
float gap1 = period - ZROD(s1);
float gap2 = ZROD(s2) - period;
float zActPeriod = ZACTUAL(s1) + (ZACTUAL(s2) - ZACTUAL(s1)) * gap1 / (gap1 + gap2);
m_scalingFactor *= period / zActPeriod;
}
// Case 3 - Both samples before period (s1 - s2 - p): extrapolate zact
else {
float gap1 = ZROD(s2) - ZROD(s1);
float gap2 = period - ZROD(s2);
float zActPeriod = ZACTUAL(s2) + (ZACTUAL(s2) - ZACTUAL(s1)) * gap2 / gap1;
m_scalingFactor *= period / zActPeriod;
}
}
}
//===========================================================================
void ZWobble::setScalingFactor(float zActualPerScaledLength) {
m_scalingFactor = zActualPerScaledLength;
}
//===========================================================================
void ZWobble::setSample(float zRod, float zActual) {
if (debugLevel & DEBUG_DEBUG) {
ECHO_SMV(DB, "New sample Rod: ", zRod);
ECHO_EMV(" Act: ", zActual);
}
if (m_puls <= 0) {
ECHO_LM(DB, "You must define a period first (M97 W...)");
return;
}
if (m_sinusoidal) {
m_sinusoidal = false;
calculateLut(); // initializes the LUT to linear
}
insertInLut(zRod, zActual);
}
//===========================================================================
void ZWobble::insertInLut(float zRod, float zActual) {
// check if the given zRod alread exists in LUT
for (int i = 0; i < lutSize; i++) {
if (EQUAL_WITHIN_TOLERANCE(zRod, ZROD(i))) {
// replace value
SET_ZROD(i, zRod);
SET_ZACTUAL(i, zActual);
return;
}
}
// ok the value does not exist: is there still space in LUT? Insert it
if (lutSize < STEPS_IN_ZLUT) {
int zPlace = -1;
for (int i = 0; i < lutSize; i++) {
if (ZROD(i) > zRod) {
zPlace = i;
break;
}
}
// shift samples after zPlace
for (int i = lutSize; i > zPlace; i--) {
SET_ZROD(i, ZROD(i - 1));
SET_ZACTUAL(i, ZACTUAL(i - 1));
}
lutSize++; // increase lutSize
// insert sample
SET_ZROD(zPlace, zRod);
SET_ZACTUAL(zPlace, zActual);
return;
}
else {
// lutSize == STEPS_IN_ZLUT: replace the closest point with the new sample
int zPlace = 0;
float dist = DISTANCE(zRod, ZROD(zPlace));
for (int i = 1; i < lutSize; i++) {
if (DISTANCE(zRod, ZROD(i)) < dist) {
zPlace = i;
dist = DISTANCE(zRod, ZROD(i));
}
}
SET_ZROD(zPlace, zRod);
SET_ZACTUAL(zPlace, zActual);
}
}
//===========================================================================
void ZWobble::initLinearLut() {
float period = TWOPI / m_puls;
lutSize = 2; // only 2 samples originally
SET_ZROD(0, 0);
SET_ZACTUAL(0, 0);
SET_ZROD(1, period);
SET_ZACTUAL(1, period);
}
//===========================================================================
// calculate the ZRod -> Zactual LUT using the model Zactual = Zrod + sin(w*Zrod) - this will actually only be used for one period
void ZWobble::calculateLut() {
lastZ = -1.0;
lastZRod = -1.0; // reinitialize memorized Z values since we are changing the model
if (!areParametersConsistent()) return;
if (!m_sinusoidal) {
initLinearLut();
return; // if the model is not sinusoidal, initializes LUT to linear
}
lutSize = STEPS_IN_ZLUT;
float period = TWOPI / m_puls;
// divide the period in STEPS_IN_ZLUT steps
float lutStep = period / STEPS_IN_ZLUT;
for (int i = 0; i < STEPS_IN_ZLUT; i++) {
float zRod = lutStep * i;
SET_ZROD(i, zRod);
SET_ZACTUAL(i, zRod + m_amplitude * sin(m_puls * zRod));
}
}
//===========================================================================
void ZWobble::setAmplitude(float _amplitude) {
m_amplitude = _amplitude;
m_sinusoidal = true; // setAmplitude sets to sinusoidal by default
calculateLut();
}
//===========================================================================
void ZWobble::setPeriod(float _period) {
if (_period <= 0) return;
m_puls = TWOPI/_period;
calculateLut();
}
//===========================================================================
void ZWobble::setPhase(float _phase ) {
// poor man's modulo operation
while (_phase > 0) _phase -= 360;
while (_phase < 0) _phase += 360;
// phase now will be between 0 and 360
m_phase = (_phase * M_PI / 180); // convert phase to radians
}
//===========================================================================
void ZWobble::ReportToSerial() {
if (!m_sinusoidal)
ECHO_SM(DB, "Custom wobble function");
else {
ECHO_SMV(DB, "ZWobble Amp(A): ", m_amplitude);
}
ECHO_MV(" phase(P): ", m_phase);
ECHO_MV(" period(W): ", TWOPI / m_puls);
ECHO_MV(" puls: ", m_puls);
if (!areParametersConsistent())
ECHO_M(" Warning! Inconsistent parameters!");
ECHO_E;
if (!m_sinusoidal) {
// print out the LUT
for (int i = 0; i < lutSize; i++) {
ECHO_SMV(DB, "Rod: ", ZROD(i));
ECHO_MV(" Act: ", ZACTUAL(i));
int delta = (ZACTUAL(i) - ZROD(i)) * 200 + 20;
for (int j = 0; j < delta; j++) {
ECHO_M(" ");
}
ECHO_EM(" +");
}
}
}
//===========================================================================
float ZWobble::findInLut(float z) {
int i = 0;
if (z >= ZACTUAL(lutSize - 1))
return ZROD(lutSize - 1);
if (z <= ZACTUAL(0))
return ZROD(0);
for (i = 0; i < lutSize; i++) {
if (ZACTUAL(i) > z)
break;
}
float invZDist = 1 / (ZACTUAL(i) - ZACTUAL(i-1)); // distance between Z steps
float interpZ = (ZROD(i - 1) * (ZACTUAL(i) - z) + ZROD(i) * (z - ZACTUAL(i - 1))) * invZDist; // linear interpolation between neighboring Z values
return interpZ;
}
//===========================================================================
// Find the Z value to be given to the "rod" in order to obtain the desired Z
float ZWobble::findZRod(float z) {
int nCycle = 0;
float identicalZ = -m_phase / m_puls;
// find the last point in which the two Z are identical: this happens every (2kPI-phase)/w
while (identicalZ <= z) identicalZ = (TWOPI * (++nCycle) - m_phase) / m_puls;
// find Z again using the previous cycle
identicalZ = (TWOPI *(nCycle - 1) - m_phase) / m_puls;
float deltaZa = z - identicalZ;
// find deltaZRod by linear interpolation of the lookup table
float deltaZrod = findInLut(deltaZa);
return identicalZ + deltaZrod;
}
//===========================================================================
// insert a plan_buffer_line if required to handle any hysteresis
void ZWobble::InsertCorrection(const float targetZ) {
if (!m_consistent) return; // don't go through consistency checks all the time; just check one bool
float originZ = (float)position[Z_AXIS] / axis_steps_per_unit[Z_AXIS];
if (originZ < ZWOBBLE_MIN_Z || targetZ < ZWOBBLE_MIN_Z) return;
if (debugLevel & DEBUG_DEBUG) {
ECHO_SMV(DB, "Origin: ", originZ);
ECHO_MV(" Target: ", targetZ);
}
if (EQUAL_WITHIN_TOLERANCE(originZ, targetZ)) return; // if there is no Z move, do nothing
float originZRod;
// there is a high chance that the origin Z is the same as the last target Z: skip one iteration of the algorithm if possible
if (originZ == lastZ)
originZRod = lastZRod;
else
originZRod = findZRod(originZ);
if (debugLevel & DEBUG_DEBUG)
ECHO_MV(" Origin rod: ", originZRod);
float targetZRod = findZRod(targetZ);
if (debugLevel & DEBUG_DEBUG)
ECHO_MV(" Target Rod: ", targetZRod);
// difference in steps between the correct movement (originZRod->targetZRod) and the planned movement
long stepDiff = lround((targetZRod - originZRod) * axis_steps_per_unit[Z_AXIS]) - (lround(targetZ * axis_steps_per_unit[Z_AXIS]) - position[Z_AXIS]);
if (debugLevel & DEBUG_DEBUG)
ECHO_EMV(" stepDiff: ", stepDiff);
lastZ = targetZ;
lastZRod = targetZRod;
// don't adjust if target posizion is less than 0
if (position[Z_AXIS] - stepDiff > 0)
position[Z_AXIS] -= stepDiff;
}
#endif
MK/module/motion/cartesian_correction.h 0 → 100644
View file @ b3cca544
/**
* cartesian_correction.h
* A class that manages hysteresis by inserting extra plan_buffer_line when necessary
* A class that manages ZWobble
*
* Copyright (c) 2016 MagoKimbra
* Copyright (c) 2013 Francesco Santini
* Copyright (c) 2012 Neil James Martin
*
* Grbl 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.
*
* Grbl 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.
*
* You should have received a copy of the GNU General Public License
* along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _CARTESIAN_CORRECTION_H
#define _CARTESIAN_CORRECTION_H
#ifdef HYSTERESIS
//===========================================================================
class Hysteresis {
public:
Hysteresis(float x_mm, float y_mm, float z_mm, float e_mm);
void Set(float x_mm, float y_mm, float z_mm, float e_mm);
void SetAxis(uint8_t axis, float mm);
void ReportToSerial();
void InsertCorrection(const float x, const float y, const float z, const float e);
private:
void calcSteps();
float m_hysteresis_mm[NUM_AXIS];
long m_hysteresis_steps[NUM_AXIS];
uint8_t m_prev_direction_bits;
uint8_t m_hysteresis_bits;
};
//===========================================================================
extern Hysteresis hysteresis;
#endif // HYSTERESIS
#ifdef ZWOBBLE
#define STEPS_IN_ZLUT 50
#define ZWOBBLE_MIN_Z 0.1
//===========================================================================
class ZWobble {
public:
ZWobble(float _amplitude, float _period, float _phase);
void Set(float _amplitude, float _period, float _phase);
void ReportToSerial();
void InsertCorrection(const float targetZ);
void setAmplitude(float _amplitude);
void setPeriod(float _period);
void setPhase(float _phase);
void setSample(float zRod, float zActual);
void setScaledSample(float zRod, float zScaledLength);
void setScalingFactor(float zActualPerScaledLength);
private:
float m_amplitude, m_puls, m_phase;
bool m_consistent;
int lutSize;
float zLut[STEPS_IN_ZLUT][2];
void calculateLut();
void initLinearLut();
void insertInLut(float, float);
float findInLut(float);
float findZRod(float);
bool areParametersConsistent();
float lastZ, lastZRod;
float m_scalingFactor;
bool m_sinusoidal;
};
//===========================================================================
extern ZWobble zwobble;
#endif // ZWOBBLE
#endif // _CARTESIAN_CORRECTION_H
MK/module/motion/planner.cpp
View file @ b3cca544
......@@ -487,6 +487,16 @@ float junction_deviation = 0.1;
void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver)
#endif // AUTO_BED_LEVELING_FEATURE
{
#if ENABLED(ZWOBBLE)
// Calculate ZWobble
zwobble.InsertCorrection(z);
#endif
#if ENABLED(HYSTERESIS)
// Calculate Hysteresis
hysteresis.InsertCorrection(x, y, z, e);
#endif
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
......
MK/module/motion/planner.h
View file @ b3cca544
......@@ -140,6 +140,7 @@ extern float max_z_jerk;
extern float max_e_jerk[EXTRUDERS];
extern float mintravelfeedrate;
extern unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
extern long position[NUM_AXIS];
#if ENABLED(AUTOTEMP)
extern bool autotemp_enabled;
......
MK/module/motion/qr_solve.cpp
View file @ b3cca544
#include "../../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(AUTO_BED_LEVELING_GRID)
#include "qr_solve.h"
#include <stdlib.h>
#include <math.h>
//# include "r8lib.h"
#include "qr_solve.h"
#include <stdlib.h>
#include <math.h>
int i4_min(int i1, int i2)
//# include "r8lib.h"
int i4_min(int i1, int i2)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
I4_MIN returns the smaller of two I4's.
......@@ -33,15 +31,15 @@ int i4_min(int i1, int i2)
Input, int I1, I2, two integers to be compared.
Output, int I4_MIN, the smaller of I1 and I2.
*/
{
*/
{
return (i1 < i2) ? i1 : i2;
}
}
double r8_epsilon(void)
double r8_epsilon(void)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
R8_EPSILON returns the R8 round off unit.
......@@ -69,16 +67,16 @@ double r8_epsilon(void)
Parameters:
Output, double R8_EPSILON, the R8 round-off unit.
*/
{
*/
{
const double value = 2.220446049250313E-016;
return value;
}
}
double r8_max(double x, double y)
double r8_max(double x, double y)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
R8_MAX returns the maximum of two R8's.
......@@ -100,15 +98,15 @@ double r8_max(double x, double y)
Input, double X, Y, the quantities to compare.
Output, double R8_MAX, the maximum of X and Y.
*/
{
*/
{
return (y < x) ? x : y;
}
}
double r8_abs(double x)
double r8_abs(double x)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
R8_ABS returns the absolute value of an R8.
......@@ -130,15 +128,15 @@ double r8_abs(double x)
Input, double X, the quantity whose absolute value is desired.
Output, double R8_ABS, the absolute value of X.
*/
{
*/
{
return (x < 0.0) ? -x : x;
}
}
double r8_sign(double x)
double r8_sign(double x)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
R8_SIGN returns the sign of an R8.
......@@ -160,15 +158,15 @@ double r8_sign(double x)
Input, double X, the number whose sign is desired.
Output, double R8_SIGN, the sign of X.
*/
{
*/
{
return (x < 0.0) ? -1.0 : 1.0;
}
}
double r8mat_amax(int m, int n, double a[])
double r8mat_amax(int m, int n, double a[])
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
......@@ -199,8 +197,8 @@ double r8mat_amax(int m, int n, double a[])
Input, double A[M*N], the M by N matrix.
Output, double R8MAT_AMAX, the maximum absolute value entry of A.
*/
{
*/
{
double value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
......@@ -208,12 +206,12 @@ double r8mat_amax(int m, int n, double a[])
}
}
return value;
}
}
void r8mat_copy(double a2[], int m, int n, double a1[])
void r8mat_copy(double a2[], int m, int n, double a1[])
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
......@@ -242,20 +240,20 @@ void r8mat_copy(double a2[], int m, int n, double a1[])
Input, double A1[M*N], the matrix to be copied.
Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
*/
{
*/
{
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++)
a2[i + j * m] = a1[i + j * m];
}
}
}
/******************************************************************************/
/******************************************************************************/
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DAXPY computes constant times a vector plus a vector.
......@@ -302,8 +300,8 @@ void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
On output, DY[*] has been replaced by DY[*] + DA * DX[*].
Input, int INCY, the increment between successive entries of DY.
*/
{
*/
{
if (n <= 0 || da == 0.0) return;
int i, ix, iy, m;
......@@ -340,13 +338,13 @@ void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
dy[i + 3] = dy[i + 3] + da * dx[i + 3];
}
}
}
/******************************************************************************/
}
/******************************************************************************/
double ddot(int n, double dx[], int incx, double dy[], int incy)
double ddot(int n, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DDOT forms the dot product of two vectors.
......@@ -393,8 +391,8 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
Output, double DDOT, the sum of the product of the corresponding
entries of DX and DY.
*/
{
*/
{
if (n <= 0) return 0.0;
......@@ -430,13 +428,13 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
}
}
return dtemp;
}
/******************************************************************************/
}
/******************************************************************************/
double dnrm2(int n, double x[], int incx)
double dnrm2(int n, double x[], int incx)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DNRM2 returns the euclidean norm of a vector.
......@@ -478,8 +476,8 @@ double dnrm2(int n, double x[], int incx)
Input, int INCX, the increment between successive entries of X.
Output, double DNRM2, the Euclidean norm of X.
*/
{
*/
{
double norm;
if (n < 1 || incx < 1)
norm = 0.0;
......@@ -503,14 +501,14 @@ double dnrm2(int n, double x[], int incx)
norm = scale * sqrt(ssq);
}
return norm;
}
/******************************************************************************/
}
/******************************************************************************/
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
int jpvt[], double qraux[])
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DQRANK computes the QR factorization of a rectangular matrix.
......@@ -579,8 +577,8 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
Output, double QRAUX[N], will contain extra information defining
the QR factorization.
*/
{
*/
{
double work[n];
for (int i = 0; i < n; i++)
......@@ -597,14 +595,14 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
return;
*kr = j + 1;
}
}
/******************************************************************************/
}
/******************************************************************************/
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DQRDC computes the QR factorization of a real rectangular matrix.
......@@ -679,8 +677,8 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
Input, int JOB, initiates column pivoting.
0, no pivoting is done.
nonzero, pivoting is done.
*/
{
*/
{
int jp;
int j;
int lup;
......@@ -791,14 +789,14 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
}
}
}
}
/******************************************************************************/
}
/******************************************************************************/
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DQRLS factors and solves a linear system in the least squares sense.
......@@ -900,8 +898,8 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
-1: LDA < M (fatal error)
-2: N < 1 (fatal error)
-3: ITASK < 1 (fatal error)
*/
{
*/
{
int ind;
if (lda < m) {
/*fprintf ( stderr, "\n" );
......@@ -938,14 +936,14 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
*/
dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
return ind;
}
/******************************************************************************/
}
/******************************************************************************/
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
double rsd[], int jpvt[], double qraux[])
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DQRLSS solves a linear system in a least squares sense.
......@@ -1013,8 +1011,8 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
Input, double QRAUX[N], auxiliary information from DQRANK
defining the QR factorization.
*/
{
*/
{
int i;
int info;
int j;
......@@ -1048,14 +1046,14 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
}
}
}
}
/******************************************************************************/
}
/******************************************************************************/
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DQRSL computes transformations, projections, and least squares solutions.
......@@ -1188,8 +1186,8 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
Output, int DQRSL, is zero unless the computation of B has
been requested and R is exactly singular. In this case, INFO is the
index of the first zero diagonal element of R, and B is left unaltered.
*/
{
*/
{
int cab;
int cb;
int cqty;
......@@ -1341,15 +1339,15 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
}
}
return info;
}
/******************************************************************************/
}
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
void dscal(int n, double sa, double x[], int incx)
void dscal(int n, double sa, double x[], int incx)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DSCAL scales a vector by a constant.
......@@ -1387,8 +1385,8 @@ void dscal(int n, double sa, double x[], int incx)
Input/output, double X[*], the vector to be scaled.
Input, int INCX, the increment between successive entries of X.
*/
{
*/
{
int i;
int ix;
int m;
......@@ -1417,14 +1415,14 @@ void dscal(int n, double sa, double x[], int incx)
ix = ix + incx;
}
}
}
/******************************************************************************/
}
/******************************************************************************/
void dswap(int n, double x[], int incx, double y[], int incy)
void dswap(int n, double x[], int incx, double y[], int incy)
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
DSWAP interchanges two vectors.
......@@ -1464,8 +1462,8 @@ void dswap(int n, double x[], int incx, double y[], int incy)
Input/output, double Y[*], one of the vectors to swap.
Input, int INCY, the increment between successive elements of Y.
*/
{
*/
{
if (n <= 0) return;
int i, ix, iy, m;
......@@ -1501,15 +1499,15 @@ void dswap(int n, double x[], int incx, double y[], int incy)
iy = iy + incy;
}
}
}
/******************************************************************************/
}
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
void qr_solve(double x[], int m, int n, double a[], double b[])
void qr_solve(double x[], int m, int n, double a[], double b[])
/******************************************************************************/
/*
/******************************************************************************/
/*
Purpose:
QR_SOLVE solves a linear system in the least squares sense.
......@@ -1554,8 +1552,8 @@ void qr_solve(double x[], int m, int n, double a[], double b[])
Input, double B[M], the right hand side.
Output, double QR_SOLVE[N], the least squares solution.
*/
{
*/
{
double a_qr[n * m], qraux[n], r[m], tol;
int ind, itask, jpvt[n], kr, lda;
......@@ -1565,7 +1563,7 @@ void qr_solve(double x[], int m, int n, double a[], double b[])
itask = 1;
ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); UNUSED(ind);
}
/******************************************************************************/
}
/******************************************************************************/
#endif
MK/module/motion/qr_solve.h
View file @ b3cca544
#if ENABLED(AUTO_BED_LEVELING_GRID)
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
double ddot(int n, double dx[], int incx, double dy[], int incy);
double dnrm2(int n, double x[], int incx);
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
double ddot(int n, double dx[], int incx, double dy[], int incy);
double dnrm2(int n, double x[], int incx);
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
int jpvt[], double qraux[]);
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job);
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask);
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
double rsd[], int jpvt[], double qraux[]);
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job);
void dscal(int n, double sa, double x[], int incx);
void dswap(int n, double x[], int incx, double y[], int incy);
void qr_solve(double x[], int m, int n, double a[], double b[]);
void dscal(int n, double sa, double x[], int incx);
void dswap(int n, double x[], int incx, double y[], int incy);
void qr_solve(double x[], int m, int n, double a[], double b[]);
#endif
MK/module/motion/vector_3.cpp
View file @ b3cca544
/*
vector_3.cpp - Vector library for bed leveling
Copyright (c) 2012 Lars Brubaker. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/**
* vector_3.cpp - Vector library for bed leveling
* Copyright (c) 2012 Lars Brubaker. All right reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "../../base.h"
#include <math.h>
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
#include "vector_3.h"
#include <math.h>
#include "vector_3.h"
vector_3::vector_3() : x(0), y(0), z(0) { }
vector_3::vector_3() : x(0), y(0), z(0) { }
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
vector_3 vector_3::cross(vector_3 left, vector_3 right) {
vector_3 vector_3::cross(vector_3 left, vector_3 right) {
return vector_3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z,
left.x * right.y - left.y * right.x);
}
}
vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
vector_3 vector_3::get_normal() {
vector_3 vector_3::get_normal() {
vector_3 normalized = vector_3(x, y, z);
normalized.normalize();
return normalized;
}
}
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
void vector_3::normalize() {
void vector_3::normalize() {
float length = get_length();
x /= length;
y /= length;
z /= length;
}
}
void vector_3::apply_rotation(matrix_3x3 matrix) {
void vector_3::apply_rotation(matrix_3x3 matrix) {
float resultX = x * matrix.matrix[0][0] + y * matrix.matrix[1][0] + z * matrix.matrix[2][0];
float resultY = x * matrix.matrix[0][1] + y * matrix.matrix[1][1] + z * matrix.matrix[2][1];
float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2];
x = resultX;
y = resultY;
z = resultZ;
}
}
void vector_3::debug(const char title[]) {
void vector_3::debug(const char title[]) {
ECHO_ST(DB, title);
ECHO_MV(" x: ", x, 6);
ECHO_MV(" y: ", y, 6);
ECHO_EMV(" z: ", z, 6);
}
}
void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
vector_3 vector = vector_3(x, y, z);
vector.apply_rotation(matrix);
x = vector.x;
y = vector.y;
z = vector.z;
}
}
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
//row_0.debug("row_0");
//row_1.debug("row_1");
//row_2.debug("row_2");
......@@ -85,15 +85,15 @@ matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3
new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z;
//new_matrix.debug("new_matrix");
return new_matrix;
}
}
void matrix_3x3::set_to_identity() {
void matrix_3x3::set_to_identity() {
matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0;
matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
}
}
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
vector_3 z_row = target.get_normal();
vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
......@@ -107,17 +107,17 @@ matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
// rot.debug("rot");
return rot;
}
}
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
matrix_3x3 new_matrix;
new_matrix.matrix[0][0] = original.matrix[0][0]; new_matrix.matrix[0][1] = original.matrix[1][0]; new_matrix.matrix[0][2] = original.matrix[2][0];
new_matrix.matrix[1][0] = original.matrix[0][1]; new_matrix.matrix[1][1] = original.matrix[1][1]; new_matrix.matrix[1][2] = original.matrix[2][1];
new_matrix.matrix[2][0] = original.matrix[0][2]; new_matrix.matrix[2][1] = original.matrix[1][2]; new_matrix.matrix[2][2] = original.matrix[2][2];
return new_matrix;
}
}
void matrix_3x3::debug(const char title[]) {
void matrix_3x3::debug(const char title[]) {
ECHO_LT(DB, title);
for (uint8_t i = 0; i < 3; i++) {
ECHO_S(DB);
......@@ -128,6 +128,6 @@ void matrix_3x3::debug(const char title[]) {
}
ECHO_E;
}
}
}
#endif // AUTO_BED_LEVELING_FEATURE
MK/module/motion/vector_3.h
View file @ b3cca544
/*
vector_3.cpp - Vector library for bed leveling
Copyright (c) 2012 Lars Brubaker. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/**
* vector_3.cpp - Vector library for bed leveling
* Copyright (c) 2012 Lars Brubaker. All right reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#ifndef VECTOR_3_H
#define VECTOR_3_H
#define VECTOR_3_H
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
class matrix_3x3;
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
class matrix_3x3;
struct vector_3 {
struct vector_3 {
float x, y, z;
vector_3();
......@@ -40,9 +40,9 @@ struct vector_3 {
void debug(const char title[]);
void apply_rotation(matrix_3x3 matrix);
};
};
struct matrix_3x3 {
struct matrix_3x3 {
float matrix[3][3];
static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
......@@ -50,12 +50,10 @@ struct matrix_3x3 {
static matrix_3x3 transpose(matrix_3x3 original);
void set_to_identity();
void debug(const char title[]);
};
};
void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
#endif // AUTO_BED_LEVELING_FEATURE
void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
#endif // AUTO_BED_LEVELING_FEATURE
#endif // VECTOR_3_H
MK/module/servo/ServoTimers.h
View file @ b3cca544
/*
Copyright (c) 2013 Arduino LLC. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/**
* Copyright (c) 2013 Arduino LLC. All right reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/*
/**
* Defines for 16 bit timers used with Servo library
*
* If _useTimerX is defined then TimerX is a 16 bit timer on the current board
......@@ -51,4 +51,3 @@ typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t;
#else // everything else
typedef enum { _Nbr_16timers } timer16_Sequence_t;
#endif
MK/module/servo/servo.cpp
View file @ b3cca544
/*
Copyright (c) 2013 Arduino LLC. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/**
* Copyright (c) 2013 Arduino LLC. All right reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "../../base.h"
#if HAS(SERVOS)
#include "servo.h"
#include "servo.h"
#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
static servo_t servos[MAX_SERVOS]; // static array of servo structures
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
static servo_t servos[MAX_SERVOS]; // static array of servo structures
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
uint8_t ServoCount = 0; // the total number of attached servos
uint8_t ServoCount = 0; // the total number of attached servos
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
/************ static functions common to all instances ***********************/
/************ static functions common to all instances ***********************/
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
if( Channel[timer] < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
......@@ -65,95 +65,95 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
}
#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
// Interrupt handlers for Arduino
#if defined(_useTimer1)
SIGNAL (TIMER1_COMPA_vect)
{
#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
// Interrupt handlers for Arduino
#if defined(_useTimer1)
SIGNAL (TIMER1_COMPA_vect)
{
handle_interrupts(_timer1, &TCNT1, &OCR1A);
}
#endif
}
#endif
#if defined(_useTimer3)
SIGNAL (TIMER3_COMPA_vect)
{
#if defined(_useTimer3)
SIGNAL (TIMER3_COMPA_vect)
{
handle_interrupts(_timer3, &TCNT3, &OCR3A);
}
#endif
}
#endif
#if defined(_useTimer4)
SIGNAL (TIMER4_COMPA_vect)
{
#if defined(_useTimer4)
SIGNAL (TIMER4_COMPA_vect)
{
handle_interrupts(_timer4, &TCNT4, &OCR4A);
}
#endif
}
#endif
#if defined(_useTimer5)
SIGNAL (TIMER5_COMPA_vect)
{
#if defined(_useTimer5)
SIGNAL (TIMER5_COMPA_vect)
{
handle_interrupts(_timer5, &TCNT5, &OCR5A);
}
#endif
}
#endif
#elif defined WIRING
// Interrupt handlers for Wiring
#if defined(_useTimer1)
void Timer1Service()
{
#elif defined WIRING
// Interrupt handlers for Wiring
#if defined(_useTimer1)
void Timer1Service()
{
handle_interrupts(_timer1, &TCNT1, &OCR1A);
}
#endif
#if defined(_useTimer3)
void Timer3Service()
{
}
#endif
#if defined(_useTimer3)
void Timer3Service()
{
handle_interrupts(_timer3, &TCNT3, &OCR3A);
}
#endif
#endif
}
#endif
#endif
static void initISR(timer16_Sequence_t timer)
{
#if defined (_useTimer1)
static void initISR(timer16_Sequence_t timer)
{
#if defined (_useTimer1)
if(timer == _timer1) {
TCCR1A = 0; // normal counting mode
TCCR1B = _BV(CS11); // set prescaler of 8
TCNT1 = 0; // clear the timer count
#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
TIFR |= _BV(OCF1A); // clear any pending interrupts;
TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
#else
#else
// here if not ATmega8 or ATmega128
TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
#endif
#if defined(WIRING)
#endif
#if defined(WIRING)
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
#endif
#endif
}
#endif
#endif
#if defined (_useTimer3)
#if defined (_useTimer3)
if(timer == _timer3) {
TCCR3A = 0; // normal counting mode
TCCR3B = _BV(CS31); // set prescaler of 8
TCNT3 = 0; // clear the timer count
#if defined(__AVR_ATmega128__)
#if defined(__AVR_ATmega128__)
TIFR |= _BV(OCF3A); // clear any pending interrupts;
ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
#else
#else
TIFR3 = _BV(OCF3A); // clear any pending interrupts;
TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
#endif
#if defined(WIRING)
#endif
#if defined(WIRING)
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
#endif
#endif
}
#endif
#endif
#if defined (_useTimer4)
#if defined (_useTimer4)
if(timer == _timer4) {
TCCR4A = 0; // normal counting mode
TCCR4B = _BV(CS41); // set prescaler of 8
......@@ -161,9 +161,9 @@ static void initISR(timer16_Sequence_t timer)
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
}
#endif
#endif
#if defined (_useTimer5)
#if defined (_useTimer5)
if(timer == _timer5) {
TCCR5A = 0; // normal counting mode
TCCR5B = _BV(CS51); // set prescaler of 8
......@@ -171,13 +171,13 @@ static void initISR(timer16_Sequence_t timer)
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
}
#endif
}
#endif
}
static void finISR(timer16_Sequence_t timer)
{
static void finISR(timer16_Sequence_t timer)
{
//disable use of the given timer
#if defined WIRING // Wiring
#if defined WIRING // Wiring
if(timer == _timer1) {
#if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
......@@ -194,25 +194,25 @@ static void finISR(timer16_Sequence_t timer)
#endif
timerDetach(TIMER3OUTCOMPAREA_INT);
}
#else
#else
//For arduino - in future: call here to a currently undefined function to reset the timer
#endif
}
#endif
}
static boolean isTimerActive(timer16_Sequence_t timer)
{
static boolean isTimerActive(timer16_Sequence_t timer)
{
// returns true if any servo is active on this timer
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
if(SERVO(timer,channel).Pin.isActive == true)
return true;
}
return false;
}
}
/****************** end of static functions ******************************/
/****************** end of static functions ******************************/
Servo::Servo() {
Servo::Servo() {
if (ServoCount < MAX_SERVOS) {
this->servoIndex = ServoCount++; // assign a servo index to this instance
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
......@@ -220,13 +220,13 @@ Servo::Servo() {
else {
this->servoIndex = INVALID_SERVO; // too many servos
}
}
}
uint8_t Servo::attach(int pin) {
uint8_t Servo::attach(int pin) {
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}
}
uint8_t Servo::attach(int pin, int min, int max) {
uint8_t Servo::attach(int pin, int min, int max) {
if (this->servoIndex >= MAX_SERVOS) return -1;
......@@ -243,22 +243,22 @@ uint8_t Servo::attach(int pin, int min, int max) {
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
return this->servoIndex;
}
}
void Servo::detach() {
void Servo::detach() {
servos[this->servoIndex].Pin.isActive = false;
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) finISR(timer);
}
}
void Servo::write(int value) {
void Servo::write(int value) {
if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
}
this->writeMicroseconds(value);
}
}
void Servo::writeMicroseconds(int value) {
void Servo::writeMicroseconds(int value) {
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if (channel < MAX_SERVOS) { // ensure channel is valid
......@@ -267,21 +267,21 @@ void Servo::writeMicroseconds(int value) {
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
CRITICAL_SECTION_START;
servo_info[channel].ticks = value;
servos[channel].ticks = value;
CRITICAL_SECTION_END;
}
}
}
// return the value as degrees
int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
// return the value as degrees
int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
int Servo::readMicroseconds() {
int Servo::readMicroseconds() {
return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
}
}
bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
void Servo::move(int value) {
void Servo::move(int value) {
if (this->attach(0) >= 0) {
this->write(value);
#if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
......@@ -289,6 +289,6 @@ void Servo::move(int value) {
this->detach();
#endif
}
}
}
#endif
MK/module/servo/servo.h
View file @ b3cca544
......@@ -45,12 +45,14 @@
detach() - Stops an attached servos from pulsing its i/o pin.
*/
#ifndef SERVO_H
#define SERVO_H
#ifndef _SERVO_H
#define _SERVO_H
#include <inttypes.h>
#if HAS(SERVOS)
#include <inttypes.h>
#include "ServoTimers.h"
/*
/*
* Defines for 16 bit timers used with Servo library
*
* If _useTimerX is defined then TimerX is a 16 bit timer on the current board
......@@ -58,32 +60,29 @@
* _Nbr_16timers indicates how many 16 bit timers are available.
*/
// Architecture specific include
#include "ServoTimers.h"
#define Servo_VERSION 2 // software version of this library
#define Servo_VERSION 2 // software version of this library
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached
#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached
#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds
#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer
#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER)
#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer
#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER)
#define INVALID_SERVO 255 // flag indicating an invalid servo index
#define INVALID_SERVO 255 // flag indicating an invalid servo index
typedef struct {
typedef struct {
uint8_t nbr :6 ; // a pin number from 0 to 63
uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false
} ServoPin_t;
} ServoPin_t;
typedef struct {
typedef struct {
ServoPin_t Pin;
volatile unsigned int ticks;
} servo_t;
} servo_t;
class Servo {
class Servo {
public:
Servo();
uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
......@@ -102,6 +101,7 @@ class Servo {
uint8_t servoIndex; // index into the channel data for this servo
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH
};
};
#endif
#endif // SERVOS
#endif // _SERVO_H
MK/module/watchdog/watchdog.cpp
View file @ b3cca544
#include "../../base.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#include "watchdog.h"
// Initialize watchdog with a 4 sec interrupt time
void watchdog_init() {
// Initialize watchdog with a 4 sec interrupt time
void watchdog_init() {
#if ENABLED(WATCHDOG_RESET_MANUAL)
// We enable the watchdog timer, but only for the interrupt.
// Take care, as this requires the correct order of operation, with interrupts disabled. See the datasheet of any AVR chip for details.
......@@ -15,19 +14,19 @@ void watchdog_init() {
#else
wdt_enable(WDTO_4S);
#endif
}
}
//===========================================================================
//=================================== ISR ===================================
//===========================================================================
//===========================================================================
//=================================== ISR ===================================
//===========================================================================
// Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
#if ENABLED(WATCHDOG_RESET_MANUAL)
// Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
#if ENABLED(WATCHDOG_RESET_MANUAL)
ISR(WDT_vect) {
ECHO_LM(ER, "Something is wrong, please turn off the printer.");
kill(PSTR("ERR:Please Reset")); //kill blocks //16 characters so it fits on a 16x2 display
while (1); //wait for user or serial reset
}
#endif //WATCHDOG_RESET_MANUAL
#endif //WATCHDOG_RESET_MANUAL
#endif //USE_WATCHDOG
MK/module/watchdog/watchdog.h
View file @ b3cca544
#ifndef WATCHDOG_H
#define WATCHDOG_H
#define WATCHDOG_H
#include <avr/wdt.h>
#include <avr/wdt.h>
// Initialize watchdog with a 4 second interrupt time
void watchdog_init();
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or AVR will go into emergency procedures.
inline void watchdog_reset() { wdt_reset(); }
// Initialize watchdog with a 4 second interrupt time
void watchdog_init();
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or AVR will go into emergency procedures.
inline void watchdog_reset() { wdt_reset(); }
#endif
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment